diff --git a/.github/dependabot.yml b/.github/dependabot.yml index f5e9921f23..f89baba15b 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -18,3 +18,17 @@ updates: schedule: interval: "weekly" target-branch: "humble" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "jazzy" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "kilted" diff --git a/.github/mergify.yml b/.github/mergify.yml index 84ad380e90..8293ec1b06 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,6 +8,24 @@ pull_request_rules: branches: - humble + - name: Backport to jazzy at reviewers discretion + conditions: + - base=master + - "label=backport-jazzy" + actions: + backport: + branches: + - jazzy + + - name: Backport to kilted at reviewers discretion + conditions: + - base=master + - "label=backport-kilted" + actions: + backport: + branches: + - kilted + - name: Ask to resolve conflict conditions: - conflict diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 4023b42a7b..03b6511233 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -17,9 +17,9 @@ on: - '**.xml' concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: abi_check: diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index 069a5240f5..c7e5aebf73 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -4,21 +4,7 @@ name: Humble Binary Build 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: + pull_request: &event branches: - humble paths: @@ -32,14 +18,15 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' - '**.xml' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: binary: diff --git a/.github/workflows/humble-check-docs.yml b/.github/workflows/humble-check-docs.yml index 448ff4084b..09b7d695e9 100644 --- a/.github/workflows/humble-check-docs.yml +++ b/.github/workflows/humble-check-docs.yml @@ -8,7 +8,14 @@ on: paths: - '**.rst' - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory + - '.github/workflows/humble-check-docs.yml' concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml index 55d4aa0cfd..c826935710 100644 --- a/.github/workflows/humble-coverage-build.yml +++ b/.github/workflows/humble-coverage-build.yml @@ -1,22 +1,7 @@ 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: + pull_request: &event branches: - humble paths: @@ -31,11 +16,12 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' - '**.xml' + push: *event concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: coverage_humble: diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index bc92501862..f8c457e24f 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -1,4 +1,4 @@ -name: Debian Humble Source Build +name: Humble - Debian Semi-Binary Build on: workflow_dispatch: pull_request: @@ -17,15 +17,15 @@ on: - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: - humble_debian: + debian_semi_binary_build: name: Humble debian build uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master with: diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml index 38a76ee025..e6cebafa83 100644 --- a/.github/workflows/humble-pre-commit.yml +++ b/.github/workflows/humble-pre-commit.yml @@ -5,6 +5,9 @@ on: pull_request: branches: - humble + push: + branches: + - humble concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/humble-pre-release.yml b/.github/workflows/humble-pre-release.yml new file mode 100644 index 0000000000..a8a4dab1a0 --- /dev/null +++ b/.github/workflows/humble-pre-release.yml @@ -0,0 +1,27 @@ +name: Humble - pre-release +# author: Christoph Froehlich + +on: + workflow_dispatch: + inputs: + downstream_depth: + description: 'The depth of the depends-on tree to be included in the overlay workspace (-1 implies unlimited, default: 0)' + required: false + default: 0 + type: number + pull_request: + branches: + - humble + types: + - opened # default + - reopened # default + - synchronize # default + - labeled # also if a label changes + +jobs: + default: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-prerelease.yml@master + with: + ros_distro: humble + # downstream_depth is not set on pull_request event + prerelease_downstream_depth: ${{ github.event_name == 'pull_request' && '0' || inputs.downstream_depth }} diff --git a/.github/workflows/humble-rhel-semi-binary-build.yml b/.github/workflows/humble-rhel-semi-binary-build.yml index 46d3dd1989..670037fa24 100644 --- a/.github/workflows/humble-rhel-semi-binary-build.yml +++ b/.github/workflows/humble-rhel-semi-binary-build.yml @@ -1,4 +1,4 @@ -name: RHEL Humble Binary Build +name: Humble - RHEL Semi-Binary Build on: workflow_dispatch: pull_request: @@ -17,15 +17,15 @@ on: - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: - humble_rhel_binary: + rhel_semi_binary_build: name: Humble RHEL binary build uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master with: diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 1fa14dfdfa..76af7b33eb 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -3,21 +3,7 @@ name: Humble Semi-Binary Build 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: + pull_request: &event branches: - humble paths: @@ -31,14 +17,15 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' - '**.xml' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' + - cron: '33 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on humble branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: semi_binary: diff --git a/.github/workflows/humble-semi-binary-downstream-build.yml b/.github/workflows/humble-semi-binary-downstream-build.yml index a9ba472ff1..aac43839b5 100644 --- a/.github/workflows/humble-semi-binary-downstream-build.yml +++ b/.github/workflows/humble-semi-binary-downstream-build.yml @@ -36,15 +36,3 @@ jobs: # we test the downstream packages, which are part of our organization downstream_workspace: ros_controls.humble.repos not_test_downstream: false - build-downstream-3rd-party: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - with: - ros_distro: humble - ros_repo: testing - ref_for_scheduled_build: humble - upstream_workspace: ros2_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 841aa86bfe..e193f04fb0 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -15,9 +15,14 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' - '**.xml' + pull_request: + branches: + - humble + paths: + - '.github/workflows/humble-source-build.yml' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' + - cron: '03 3 * * MON-FRI' jobs: source: @@ -25,5 +30,4 @@ jobs: with: ros_distro: humble ref: humble - ros2_repo_branch: humble - os_name: ubuntu-22.04 + container: ubuntu:22.04 diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml index 82fadb55a1..527fd59738 100644 --- a/.github/workflows/jazzy-abi-compatibility.yml +++ b/.github/workflows/jazzy-abi-compatibility.yml @@ -3,7 +3,7 @@ on: workflow_dispatch: pull_request: branches: - - master + - jazzy paths: - '**.hpp' - '**.h' @@ -16,9 +16,9 @@ on: - '**.xml' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: abi_check: diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml index fb68d930e4..70b0f364f9 100644 --- a/.github/workflows/jazzy-binary-build.yml +++ b/.github/workflows/jazzy-binary-build.yml @@ -4,9 +4,9 @@ name: Jazzy Binary Build on: workflow_dispatch: - pull_request: + pull_request: &event branches: - - master + - jazzy - '*feature*' - '*feature/**' paths: @@ -20,27 +20,15 @@ on: - '**/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' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: binary: @@ -53,4 +41,4 @@ jobs: ros_distro: jazzy ros_repo: ${{ matrix.ROS_REPO }} upstream_workspace: ros2_controllers-not-released.jazzy.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: jazzy diff --git a/.github/workflows/jazzy-check-docs.yml b/.github/workflows/jazzy-check-docs.yml index cfc67a3eac..02c6f2af50 100644 --- a/.github/workflows/jazzy-check-docs.yml +++ b/.github/workflows/jazzy-check-docs.yml @@ -4,11 +4,18 @@ on: workflow_dispatch: pull_request: branches: - - master + - jazzy paths: - '**.rst' - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory + - '.github/workflows/jazzy-check-docs.yml' concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/jazzy-coverage-build.yml b/.github/workflows/jazzy-coverage-build.yml index 2f587c7003..29ff6aba44 100644 --- a/.github/workflows/jazzy-coverage-build.yml +++ b/.github/workflows/jazzy-coverage-build.yml @@ -1,42 +1,27 @@ 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' + pull_request: &event + branches: + - jazzy + 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' + push: *event concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: coverage_jazzy: diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml index ef1b42da8d..4fe2bb0dea 100644 --- a/.github/workflows/jazzy-debian-build.yml +++ b/.github/workflows/jazzy-debian-build.yml @@ -1,9 +1,9 @@ -name: Debian Jazzy Source Build +name: Jazzy - Debian Semi-Binary Build on: workflow_dispatch: pull_request: branches: - - master + - jazzy paths: - '**.hpp' - '**.h' @@ -17,19 +17,19 @@ on: - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: - jazzy_debian: + debian_semi_binary_build: 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 + ref_for_scheduled_build: jazzy skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/jazzy-pre-commit.yml b/.github/workflows/jazzy-pre-commit.yml index aab5ba52ac..5f94855963 100644 --- a/.github/workflows/jazzy-pre-commit.yml +++ b/.github/workflows/jazzy-pre-commit.yml @@ -4,7 +4,10 @@ on: workflow_dispatch: pull_request: branches: - - master + - jazzy + push: + branches: + - jazzy concurrency: group: ${{ github.workflow }}-${{ github.ref }} diff --git a/.github/workflows/jazzy-pre-release.yml b/.github/workflows/jazzy-pre-release.yml new file mode 100644 index 0000000000..0ab61dfcd9 --- /dev/null +++ b/.github/workflows/jazzy-pre-release.yml @@ -0,0 +1,27 @@ +name: Jazzy - pre-release +# author: Christoph Froehlich + +on: + workflow_dispatch: + inputs: + downstream_depth: + description: 'The depth of the depends-on tree to be included in the overlay workspace (-1 implies unlimited, default: 0)' + required: false + default: 0 + type: number + pull_request: + branches: + - jazzy + types: + - opened # default + - reopened # default + - synchronize # default + - labeled # also if a label changes + +jobs: + default: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-prerelease.yml@master + with: + ros_distro: jazzy + # downstream_depth is not set on pull_request event + prerelease_downstream_depth: ${{ github.event_name == 'pull_request' && '0' || inputs.downstream_depth }} diff --git a/.github/workflows/jazzy-rhel-semi-binary-build.yml b/.github/workflows/jazzy-rhel-semi-binary-build.yml index ed09d0efb9..113dbe636a 100644 --- a/.github/workflows/jazzy-rhel-semi-binary-build.yml +++ b/.github/workflows/jazzy-rhel-semi-binary-build.yml @@ -1,9 +1,9 @@ -name: RHEL Jazzy Semi-Binary Build +name: Jazzy - RHEL Semi-Binary Build on: workflow_dispatch: pull_request: branches: - - master + - jazzy paths: - '**.hpp' - '**.h' @@ -17,18 +17,18 @@ on: - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: - jazzy_rhel: + rhel_semi_binary_build: 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 + ref_for_scheduled_build: jazzy skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index 5a39ea5393..5adf2f413d 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -3,9 +3,9 @@ name: Jazzy Semi-Binary Build on: workflow_dispatch: - pull_request: + pull_request: &event branches: - - master + - jazzy - '*feature*' - '*feature/**' paths: @@ -19,28 +19,15 @@ on: - '**/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' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' + - cron: '33 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: semi_binary: @@ -49,14 +36,14 @@ jobs: ros_distro: jazzy ros_repo: testing upstream_workspace: ros2_controllers.jazzy.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: jazzy semi_binary_clang: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master with: ros_distro: jazzy ros_repo: testing upstream_workspace: ros2_controllers.jazzy.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: jazzy additional_debs: clang c_compiler: clang cxx_compiler: clang++ diff --git a/.github/workflows/jazzy-semi-binary-downstream-build.yml b/.github/workflows/jazzy-semi-binary-downstream-build.yml index a2db0ef7b6..62ba27bedd 100644 --- a/.github/workflows/jazzy-semi-binary-downstream-build.yml +++ b/.github/workflows/jazzy-semi-binary-downstream-build.yml @@ -6,7 +6,7 @@ on: workflow_dispatch: pull_request: branches: - - master + - jazzy paths: - '**.hpp' - '**.h' @@ -29,22 +29,10 @@ jobs: with: ros_distro: jazzy ros_repo: testing - ref_for_scheduled_build: master + ref_for_scheduled_build: jazzy 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 index 76c9bd2892..277407456e 100644 --- a/.github/workflows/jazzy-source-build.yml +++ b/.github/workflows/jazzy-source-build.yml @@ -3,7 +3,7 @@ on: workflow_dispatch: push: branches: - - master + - jazzy paths: - '**.hpp' - '**.h' @@ -15,15 +15,19 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.jazzy.repos' - '**.xml' + pull_request: + branches: + - jazzy + paths: + - '.github/workflows/jazzy-source-build.yml' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' + - cron: '03 3 * * MON-FRI' jobs: source: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master with: ros_distro: jazzy - ref: master - ros2_repo_branch: jazzy + ref: jazzy container: ubuntu:24.04 diff --git a/.github/workflows/kilted-abi-compatibility.yml b/.github/workflows/kilted-abi-compatibility.yml new file mode 100644 index 0000000000..155013c64d --- /dev/null +++ b/.github/workflows/kilted-abi-compatibility.yml @@ -0,0 +1,27 @@ +name: Kilted - ABI Compatibility Check +on: + workflow_dispatch: + pull_request: + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/kilted-abi-compatibility.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.kilted.repos' + - '**.xml' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + abi_check: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master + with: + ros_distro: kilted diff --git a/.github/workflows/kilted-binary-build.yml b/.github/workflows/kilted-binary-build.yml new file mode 100644 index 0000000000..816b0fa065 --- /dev/null +++ b/.github/workflows/kilted-binary-build.yml @@ -0,0 +1,44 @@ +name: Kilted Binary Build +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + pull_request: &event + branches: + - kilted + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.kilted.repos' + - '**.xml' + push: *event + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * MON-FRI' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_REPO: [main, testing] + with: + ros_distro: kilted + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers-not-released.kilted.repos + ref_for_scheduled_build: kilted diff --git a/.github/workflows/kilted-check-docs.yml b/.github/workflows/kilted-check-docs.yml new file mode 100644 index 0000000000..189b6df699 --- /dev/null +++ b/.github/workflows/kilted-check-docs.yml @@ -0,0 +1,29 @@ +name: Kilted Check Docs + +on: + workflow_dispatch: + pull_request: + branches: + - kilted + paths: + - '**.rst' + - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' + - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory + - '.github/workflows/kilted-check-docs.yml' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@kilted + with: + ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/kilted-coverage-build.yml b/.github/workflows/kilted-coverage-build.yml new file mode 100644 index 0000000000..2cb96a1be7 --- /dev/null +++ b/.github/workflows/kilted-coverage-build.yml @@ -0,0 +1,31 @@ +name: Coverage Build - Kilted +on: + workflow_dispatch: + pull_request: &event + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/kilted-coverage-build.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.kilted.repos' + push: *event + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + coverage_kilted: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: kilted diff --git a/.github/workflows/kilted-debian-build.yml b/.github/workflows/kilted-debian-build.yml new file mode 100644 index 0000000000..35fb0d0110 --- /dev/null +++ b/.github/workflows/kilted-debian-build.yml @@ -0,0 +1,35 @@ +name: Kilted - Debian Semi-Binary Build +on: + workflow_dispatch: + pull_request: + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.kilted.repos' + - '**.xml' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * MON-FRI' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + debian_semi_binary_build: + name: Rolling debian build + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master + with: + ros_distro: kilted + upstream_workspace: ros2_controllers.kilted.repos + ref_for_scheduled_build: kilted + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/kilted-pre-commit.yml b/.github/workflows/kilted-pre-commit.yml new file mode 100644 index 0000000000..fc2d6d0858 --- /dev/null +++ b/.github/workflows/kilted-pre-commit.yml @@ -0,0 +1,20 @@ +name: Pre-Commit - Kilted + +on: + workflow_dispatch: + pull_request: + branches: + - kilted + push: + branches: + - kilted + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: kilted diff --git a/.github/workflows/kilted-pre-release.yml b/.github/workflows/kilted-pre-release.yml new file mode 100644 index 0000000000..c2c6b4042b --- /dev/null +++ b/.github/workflows/kilted-pre-release.yml @@ -0,0 +1,27 @@ +name: Kilted - pre-release +# author: Christoph Froehlich + +on: + workflow_dispatch: + inputs: + downstream_depth: + description: 'The depth of the depends-on tree to be included in the overlay workspace (-1 implies unlimited, default: 0)' + required: false + default: 0 + type: number + pull_request: + branches: + - kilted + types: + - opened # default + - reopened # default + - synchronize # default + - labeled # also if a label changes + +jobs: + default: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-prerelease.yml@master + with: + ros_distro: kilted + # downstream_depth is not set on pull_request event + prerelease_downstream_depth: ${{ github.event_name == 'pull_request' && '0' || inputs.downstream_depth }} diff --git a/.github/workflows/kilted-rhel-semi-binary-build.yml b/.github/workflows/kilted-rhel-semi-binary-build.yml new file mode 100644 index 0000000000..79b48aa2cd --- /dev/null +++ b/.github/workflows/kilted-rhel-semi-binary-build.yml @@ -0,0 +1,34 @@ +name: Kilted - RHEL Semi-Binary Build +on: + workflow_dispatch: + pull_request: + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-rhel-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.kilted.repos' + - '**.xml' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * MON-FRI' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + rhel_semi_binary_build: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master + with: + ros_distro: kilted + upstream_workspace: ros2_controllers.kilted.repos + ref_for_scheduled_build: kilted + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/kilted-semi-binary-build.yml b/.github/workflows/kilted-semi-binary-build.yml new file mode 100644 index 0000000000..cffc6eab28 --- /dev/null +++ b/.github/workflows/kilted-semi-binary-build.yml @@ -0,0 +1,50 @@ +name: Kilted Semi-Binary Build +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + pull_request: &event + branches: + - kilted + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.kilted.repos' + - '**.xml' + push: *event + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * MON-FRI' + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + semi_binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: kilted + ros_repo: testing + upstream_workspace: ros2_controllers.kilted.repos + ref_for_scheduled_build: kilted + semi_binary_clang: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: kilted + ros_repo: testing + upstream_workspace: ros2_controllers.kilted.repos + ref_for_scheduled_build: kilted + additional_debs: clang + c_compiler: clang + cxx_compiler: clang++ + not_test_build: true diff --git a/.github/workflows/kilted-semi-binary-downstream-build.yml b/.github/workflows/kilted-semi-binary-downstream-build.yml new file mode 100644 index 0000000000..9ad1f6cb94 --- /dev/null +++ b/.github/workflows/kilted-semi-binary-downstream-build.yml @@ -0,0 +1,38 @@ +name: Kilted Downstream Build +# description: 'Build & test downstream packages from source.' +# author: Christoph Froehlich + +on: + workflow_dispatch: + pull_request: + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-semi-binary-downstream-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros_controls.kilted.repos' + - '**.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: kilted + ros_repo: testing + ref_for_scheduled_build: kilted + upstream_workspace: ros2_controllers.kilted.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.kilted.repos + not_test_downstream: false diff --git a/.github/workflows/kilted-source-build.yml b/.github/workflows/kilted-source-build.yml new file mode 100644 index 0000000000..76a2dab12d --- /dev/null +++ b/.github/workflows/kilted-source-build.yml @@ -0,0 +1,33 @@ +name: Kilted Source Build +on: + workflow_dispatch: + push: + branches: + - kilted + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/kilted-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.kilted.repos' + - '**.xml' + pull_request: + branches: + - kilted + paths: + - '.github/workflows/kilted-source-build.yml' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 3 * * MON-FRI' + +jobs: + source: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master + with: + ros_distro: kilted + ref: kilted + container: ubuntu:24.04 diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml deleted file mode 100644 index 182df6e22b..0000000000 --- a/.github/workflows/prerelease-check.yml +++ /dev/null @@ -1,37 +0,0 @@ -name: Pre-Release Check - -on: - workflow_dispatch: - inputs: - ros_distro: - description: 'Chose ROS distribution' - required: true - default: 'rolling' - type: choice - options: - - humble - - iron - - rolling - branch: - description: 'Chose branch for distro' - required: true - default: 'master' - type: choice - options: - - humble - - iron - - master - -jobs: - pre_release: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - with: - ref: ${{ github.event.inputs.branch }} - - name: industrial_ci - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: ${{ github.event.inputs.ros_distro }} - PRERELEASE: true - BASEDIR: ${{ github.workspace }}/.work diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index e3cd45c970..20afed10b2 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -13,15 +13,20 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.rolling.repos' + - 'ros2_controllers-not-released.kilted.repos' - '**.xml' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: abi_check: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-abi-check.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] with: - ros_distro: rolling + ros_distro: ${{ matrix.ROS_DISTRO }} diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index 1fd126f43c..61a201d097 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -4,7 +4,7 @@ name: Rolling Binary Build on: workflow_dispatch: - pull_request: + pull_request: &event branches: - master - '*feature*' @@ -19,29 +19,17 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.rolling.repos' + - 'ros2_controllers-not-released.kilted.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' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: binary: @@ -49,9 +37,10 @@ jobs: strategy: fail-fast: false matrix: + ROS_DISTRO: [rolling] ROS_REPO: [main, testing] with: - ros_distro: rolling + ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_controllers-not-released.rolling.repos + upstream_workspace: ros2_controllers-not-released.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml index 1144c4e6c0..56a785475d 100644 --- a/.github/workflows/rolling-check-docs.yml +++ b/.github/workflows/rolling-check-docs.yml @@ -8,7 +8,13 @@ on: paths: - '**.rst' - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory - '.github/workflows/rolling-check-docs.yml' concurrency: diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml index 54f85ecf4b..f7884cf804 100644 --- a/.github/workflows/rolling-compatibility-humble-binary-build.yml +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -4,7 +4,7 @@ name: Check Rolling Compatibility on Humble on: workflow_dispatch: - pull_request: + pull_request: &event branches: - master - '*feature*' @@ -20,25 +20,12 @@ on: - '**/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' + push: *event concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: build-on-humble: diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml index a3f406fe87..9bd04e2ad0 100644 --- a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -4,7 +4,7 @@ name: Check Rolling Compatibility on Jazzy on: workflow_dispatch: - pull_request: + pull_request: &event branches: - master - '*feature*' @@ -20,25 +20,12 @@ on: - '**/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' + push: *event concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: build-on-jazzy: diff --git a/.github/workflows/rolling-compatibility-kilted-binary-build.yml b/.github/workflows/rolling-compatibility-kilted-binary-build.yml new file mode 100644 index 0000000000..1d8c3b3bb2 --- /dev/null +++ b/.github/workflows/rolling-compatibility-kilted-binary-build.yml @@ -0,0 +1,37 @@ +name: Check Rolling Compatibility on Kilted +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Kilted distro.' + +on: + workflow_dispatch: + pull_request: &event + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-kilted-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + - '**.xml' + push: *event + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + build-on-kilted: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + with: + ros_distro: kilted + ros_repo: testing + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml index cd40b1954d..906f710d7d 100644 --- a/.github/workflows/rolling-coverage-build.yml +++ b/.github/workflows/rolling-coverage-build.yml @@ -1,22 +1,7 @@ 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: + pull_request: &event branches: - master paths: @@ -31,11 +16,12 @@ on: - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' - '**.xml' + push: *event concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: coverage_rolling: diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index 23c9aaf834..7ffcc0093f 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -1,4 +1,4 @@ -name: Debian Rolling Source Build +name: Rolling - Debian Semi-Binary Build on: workflow_dispatch: pull_request: @@ -14,22 +14,27 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - 'ros2_controllers.kilted.repos' - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: - rolling_debian: + debian_semi_binary_build: name: Rolling debian build uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] with: - ros_distro: rolling - upstream_workspace: ros2_controllers.rolling.repos + ros_distro: ${{ matrix.ROS_DISTRO }} + upstream_workspace: ros2_controllers.${{ matrix.ROS_DISTRO }}.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 index 792278d6d2..54d7e90adf 100644 --- a/.github/workflows/rolling-pre-commit.yml +++ b/.github/workflows/rolling-pre-commit.yml @@ -5,6 +5,9 @@ on: pull_request: branches: - master + push: + branches: + - master concurrency: group: ${{ github.workflow }}-${{ github.ref }} @@ -13,5 +16,9 @@ concurrency: jobs: pre-commit: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] with: - ros_distro: rolling + ros_distro: ${{ matrix.ROS_DISTRO }} diff --git a/.github/workflows/rolling-pre-release.yml b/.github/workflows/rolling-pre-release.yml new file mode 100644 index 0000000000..3550e647f1 --- /dev/null +++ b/.github/workflows/rolling-pre-release.yml @@ -0,0 +1,32 @@ +name: Rolling - pre-release +# author: Christoph Froehlich + +on: + workflow_dispatch: + inputs: + downstream_depth: + description: 'The depth of the depends-on tree to be included in the overlay workspace (-1 implies unlimited, default: 0)' + required: false + default: 0 + type: number + pull_request: + branches: + - master + types: + - opened # default + - reopened # default + - synchronize # default + - labeled # also if a label changes + +jobs: + default: + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] + + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-prerelease.yml@master + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + # downstream_depth is not set on pull_request event + prerelease_downstream_depth: ${{ github.event_name == 'pull_request' && '0' || inputs.downstream_depth }} diff --git a/.github/workflows/rolling-rhel-semi-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml index c1abdb3c5c..208f2407ba 100644 --- a/.github/workflows/rolling-rhel-semi-binary-build.yml +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -1,4 +1,4 @@ -name: RHEL Rolling Semi-Binary Build +name: Rolling - RHEL Semi-Binary Build on: workflow_dispatch: pull_request: @@ -14,21 +14,26 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - 'ros2_controllers.kilted.repos' - '**.xml' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: - rolling_rhel: + rhel_semi_binary_build: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] with: - ros_distro: rolling - upstream_workspace: ros2_controllers.rolling.repos + ros_distro: ${{ matrix.ROS_DISTRO }} + upstream_workspace: ros2_controllers.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: master skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/rolling-semi-binary-build-win.yml b/.github/workflows/rolling-semi-binary-build-win.yml new file mode 100644 index 0000000000..88fa295207 --- /dev/null +++ b/.github/workflows/rolling-semi-binary-build-win.yml @@ -0,0 +1,32 @@ +name: Rolling Windows Semi-Binary Build +# author: Christoph Fröhlich +# description: 'Build & test all dependencies from semi-binary packages.' + +on: + workflow_dispatch: + pull_request: &event + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '.github/workflows/rolling-semi-binary-build-win.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + push: *event + +concurrency: + # cancel previous runs of the same workflow, except for pushes on given branches + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} + +jobs: + binary-windows: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-win-build.yml@master + with: + ros_distro: rolling + pixi_dependencies: typeguard jinja2 boost compilers + ninja_packages: rsl + target_cmake_args: -DBUILD_TESTING=OFF diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 6521e90b5c..4be9be1678 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -3,7 +3,7 @@ name: Rolling Semi-Binary Build on: workflow_dispatch: - pull_request: + pull_request: &event branches: - master - '*feature*' @@ -18,44 +18,40 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - 'ros2_controllers.kilted.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' + push: *event schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' + - cron: '33 1 * * MON-FRI' concurrency: - # cancel previous runs of the same workflow, except for pushes on master branch + # cancel previous runs of the same workflow, except for pushes on given branches group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: ${{ !startsWith(github.ref, '/refs/heads') }} + cancel-in-progress: ${{ !startsWith(github.ref, 'refs/heads') }} jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] with: - ros_distro: rolling + ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: testing - upstream_workspace: ros2_controllers.rolling.repos + upstream_workspace: ros2_controllers.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: master semi_binary_clang: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] with: - ros_distro: rolling + ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: testing - upstream_workspace: ros2_controllers.rolling.repos + upstream_workspace: ros2_controllers.${{ matrix.ROS_DISTRO }}.repos ref_for_scheduled_build: master additional_debs: clang c_compiler: clang diff --git a/.github/workflows/rolling-semi-binary-downstream-build.yml b/.github/workflows/rolling-semi-binary-downstream-build.yml index 02ed08578b..317164c4ed 100644 --- a/.github/workflows/rolling-semi-binary-downstream-build.yml +++ b/.github/workflows/rolling-semi-binary-downstream-build.yml @@ -17,6 +17,7 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros_controls.rolling.repos' + - 'ros_controls.kilted.repos' - '**.xml' concurrency: @@ -40,19 +41,3 @@ jobs: # we test the downstream packages, which are part of our organization downstream_workspace: ros_controls.${{ matrix.ROS_DISTRO }}.repos not_test_downstream: false - build-downstream-3rd-party: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [rolling] - with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: testing - ref_for_scheduled_build: master - upstream_workspace: ros2_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 e409150974..34478d1ff7 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -14,16 +14,25 @@ on: - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.rolling.repos' + - 'ros2_controllers.kilted.repos' - '**.xml' + pull_request: + branches: + - master + paths: + - '.github/workflows/rolling-source-build.yml' schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' + - cron: '03 3 * * MON-FRI' jobs: source: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [rolling] with: - ros_distro: rolling + ros_distro: ${{ matrix.ROS_DISTRO }} ref: master - ros2_repo_branch: rolling container: ubuntu:24.04 diff --git a/.github/workflows/stale.yml b/.github/workflows/stale.yml index f30de29b75..75b6b01885 100644 --- a/.github/workflows/stale.yml +++ b/.github/workflows/stale.yml @@ -12,7 +12,7 @@ jobs: issues: write pull-requests: write steps: - - uses: actions/stale@v9 + - uses: actions/stale@v10 with: stale-issue-label: 'stale' stale-pr-label: 'stale' diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ee0e31793a..fb11912667 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v5.0.0 + rev: v6.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -29,6 +29,7 @@ repos: args: ["--allow-multiple-documents"] - id: debug-statements - id: end-of-file-fixer + exclude_types: [svg] - id: mixed-line-ending - id: trailing-whitespace exclude_types: [rst] @@ -37,7 +38,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.19.1 + rev: v3.21.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -50,20 +51,20 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 25.1.0 + rev: 25.9.0 hooks: - id: black args: ["--line-length=99"] - repo: https://github.com/pycqa/flake8 - rev: 7.2.0 + rev: 7.3.0 hooks: - id: flake8 args: ["--extend-ignore=E501"] # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v20.1.3 + rev: v21.1.2 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -109,7 +110,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.1.2 + rev: v2.0.0 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -133,7 +134,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.33.0 + rev: 0.34.1 hooks: - id: check-github-workflows args: ["--verbose"] diff --git a/README.md b/README.md index bbc9d52c34..7eaed18d41 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ros2_controllers [![License](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/graph/badge.svg?token=KSdY0tsHm6)](https://codecov.io/gh/ros-controls/ros2_controllers) +[![codecov](https://codecov.io/gh/ros-controls/ros2_controllers/branch/jazzy/graph/badge.svg?token=KSdY0tsHm6)](https://codecov.io/gh/ros-controls/ros2_controllers) Commonly used and generalized controllers for ros2-control framework that are ready to use with many robots, MoveIt2 and Nav2. @@ -14,9 +14,10 @@ If you are new to the project, please read the [contributing guide](https://cont ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**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)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__ros2_controllers__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__ros2_controllers__ubuntu_noble_amd64/) | [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)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Jdev__ros2_controllers__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Jdev__ros2_controllers__ubuntu_noble_amd64/) | [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)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Hdev__ros2_controllers__ubuntu_jammy_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Hdev__ros2_controllers__ubuntu_jammy_amd64/) | [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) +**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)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Rdev__ros2_controllers__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Rdev__ros2_controllers__ubuntu_noble_amd64/) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary/) +**Kilted** | [`kilted`](https://github.com/ros-controls/ros2_controllers/tree/kilted) | [![Kilted Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/kilted-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/kilted-binary-build.yml?branch=master)
[![Kilted Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/kilted-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/kilted-semi-binary-build.yml?branch=master)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Kdev__ros2_controllers__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Kdev__ros2_controllers__ubuntu_noble_amd64/) | [control.ros.org](https://control.ros.org/kilted/doc/ros2_controllers/doc/controllers_index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Kbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Kbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary/) +**Jazzy** | [`jazzy`](https://github.com/ros-controls/ros2_controllers/tree/jazzy) | [![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)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Jdev__ros2_controllers__ubuntu_noble_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Jdev__ros2_controllers__ubuntu_noble_amd64/) | [control.ros.org](https://control.ros.org/jazzy/doc/ros2_controllers/doc/controllers_index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__ros2_controllers__ubuntu_noble_amd64__binary/) +**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)
[![build.ros2.org](https://build.ros2.org/buildStatus/icon?job=Hdev__ros2_controllers__ubuntu_jammy_amd64&subject=build.ros2.org)](https://build.ros2.org/job/Hdev__ros2_controllers__ubuntu_jammy_amd64/) | [control.ros.org](https://control.ros.org/humble/doc/ros2_controllers/doc/controllers_index.html) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__ros2_controllers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__ros2_controllers__ubuntu_jammy_amd64__binary/) ## Acknowledgements diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 03397c56ac..e4c20d1bd4 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Rename ackermann msg to controller state msg type (`#1662 `_) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 3f49ab86ee..d9dca4c145 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -40,8 +40,15 @@ target_include_directories(ackermann_steering_controller PUBLIC $ $) target_link_libraries(ackermann_steering_controller PUBLIC - ackermann_steering_controller_parameters) -ament_target_dependencies(ackermann_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + ackermann_steering_controller_parameters + steering_controllers_library::steering_controllers_library + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${std_srvs_TARGETS}) pluginlib_export_plugin_description_file( controller_interface ackermann_steering_controller.xml) @@ -55,10 +62,10 @@ if(BUILD_TESTING) add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") ament_add_gmock(test_load_ackermann_steering_controller test/test_load_ackermann_steering_controller.cpp) - ament_target_dependencies(test_load_ackermann_steering_controller - controller_manager - hardware_interface - ros2_control_test_assets + target_link_libraries(test_load_ackermann_steering_controller + controller_manager::controller_manager + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets ) add_rostest_with_parameters_gmock(test_ackermann_steering_controller @@ -67,22 +74,12 @@ if(BUILD_TESTING) ) 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_preceding test/test_ackermann_steering_controller_preceding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceding_params.yaml) target_include_directories(test_ackermann_steering_controller_preceding PRIVATE include) target_link_libraries(test_ackermann_steering_controller_preceding ackermann_steering_controller) - ament_target_dependencies( - test_ackermann_steering_controller_preceding - controller_interface - hardware_interface - ) endif() install( diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index fc0eba497d..5fcb373f58 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.24.0 + 4.33.1 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Bence Magyar diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 9452339a81..9300fe1721 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -114,18 +114,39 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period) { + auto logger = get_node()->get_logger(); + 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(); + const auto traction_right_wheel_value_op = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional(); + const auto traction_left_wheel_value_op = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional(); + const auto steering_right_position_op = + state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional(); + const auto steering_left_position_op = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional(); + + if ( + !traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() || + !steering_right_position_op.has_value() || !steering_left_position_op.has_value()) + { + RCLCPP_DEBUG( + logger, + "Unable to retrieve the data from right wheel or left wheel or right steering position or " + "left steering position!"); + + return true; + } + + const double traction_right_wheel_value = traction_right_wheel_value_op.value(); + const double traction_left_wheel_value = traction_left_wheel_value_op.value(); + const double steering_right_position = steering_right_position_op.value(); + const double steering_left_position = steering_left_position_op.value(); + if ( std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && std::isfinite(steering_right_position) && std::isfinite(steering_left_position)) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index f7cdca092f..c151af1ecc 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -103,13 +103,13 @@ TEST_F(AckermannSteeringControllerTest, activate_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)); + auto msg = controller_->input_ref_.get(); + 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) @@ -140,9 +140,9 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success) 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_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -161,11 +161,11 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) 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); + ControllerReferenceMsg msg; + msg.header.stamp = controller_->get_node()->now(); + msg.twist.linear.x = 0.1; + msg.twist.angular.z = 0.2; + controller_->input_ref_.set(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -173,19 +173,19 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) // 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); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -213,19 +213,19 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) // 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); + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -249,8 +249,9 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat 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); + // never received a valid command, linear velocity should have been reset + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.0); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 0.0); EXPECT_EQ(msg.steering_angle_command[0], 2.2); EXPECT_EQ(msg.steering_angle_command[1], 4.4); @@ -263,17 +264,17 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat // 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); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(), + 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 3e0971c8df..8bb4a55a1b 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,54 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- +* Fix temporary copies of other semantic components (backport `#1905 `_) (`#1908 `_) +* Contributors: mergify[bot] + +4.31.0 (2025-08-27) +------------------- +* chore: tf2_ros to hpp headers (backport `#1866 `_) (`#1869 `_) +* Contributors: mergify[bot] + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- +* Fix `child_frame_id` in controller_state_msg (backport `#1601 `_) (`#1835 `_) +* Contributors: mergify[bot] + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index c71bf11bfb..6cd1ba6385 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -46,10 +46,26 @@ 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}) + admittance_controller_parameters + Eigen3::Eigen + control_toolbox::control_toolbox + controller_interface::controller_interface + hardware_interface::hardware_interface + kinematics_interface::kinematics_interface + pluginlib::pluginlib + rclcpp::rclcpp + angles::angles + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + tf2::tf2 + tf2_eigen::tf2_eigen + tf2_geometry_msgs::tf2_geometry_msgs + tf2_kdl::tf2_kdl + tf2_ros::tf2_ros + ${geometry_msgs_TARGETS} + ${trajectory_msgs_TARGETS} + ${control_msgs_TARGETS} + ${tf2_geometry_msgs_TARGETS}) pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml) @@ -64,10 +80,10 @@ if(BUILD_TESTING) # test loading admittance controller add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") ament_add_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp) - ament_target_dependencies(test_load_admittance_controller - controller_manager - hardware_interface - ros2_control_test_assets + target_link_libraries(test_load_admittance_controller + controller_manager::controller_manager + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets ) # test admittance controller function @@ -75,13 +91,9 @@ if(BUILD_TESTING) 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 - control_msgs - controller_interface - hardware_interface - ros2_control_test_assets - ) + target_link_libraries(test_admittance_controller + admittance_controller + ros2_control_test_assets::ros2_control_test_assets) endif() install( diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 23aad881c6..51da1fc848 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -31,8 +31,8 @@ #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "semantic_components/force_torque_sensor.hpp" namespace admittance_controller @@ -125,14 +125,16 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // admittance parameters std::shared_ptr parameter_handler_; - // ROS messages - std::shared_ptr joint_command_msg_; - - // real-time buffer - realtime_tools::RealtimeBuffer> + // real-time boxes + realtime_tools::RealtimeThreadSafeBox input_joint_command_; - realtime_tools::RealtimeBuffer input_wrench_command_; + realtime_tools::RealtimeThreadSafeBox input_wrench_command_; std::unique_ptr> state_publisher_; + ControllerStateMsg state_msg_; + + // save the last commands in case of unable to get value from box + trajectory_msgs::msg::JointTrajectoryPoint joint_command_msg_; + geometry_msgs::msg::WrenchStamped wrench_command_msg_; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; trajectory_msgs::msg::JointTrajectoryPoint last_reference_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 859c7e3336..7e3a289e8a 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -102,8 +102,6 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) 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; - state_message_.admittance_position.header.frame_id = parameters_.kinematics.base; - state_message_.admittance_position.child_frame_id = "admittance_offset"; // reset admittance state admittance_state_ = AdmittanceState(num_joints); @@ -392,10 +390,12 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control admittance_state_.admittance_acceleration[5]; state_message_.admittance_position = tf2::eigenToTransform(admittance_state_.admittance_position); + state_message_.admittance_position.header.frame_id = parameters_.kinematics.base; + state_message_.admittance_position.child_frame_id = "admittance_offset"; - state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base; - state_message_.ref_trans_base_ft.header.frame_id = "ft_reference"; state_message_.ref_trans_base_ft = tf2::eigenToTransform(admittance_state_.ref_trans_base_ft); + state_message_.ref_trans_base_ft.header.frame_id = parameters_.kinematics.base; + state_message_.ref_trans_base_ft.child_frame_id = parameters_.ft_sensor.frame.id; Eigen::Quaterniond quat(admittance_state_.rot_base_control); state_message_.rot_base_control.w = quat.w(); diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index cb5201424e..9f0780cada 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.24.0 + 4.33.1 Implementation of admittance controllers for different input and output interface. Bence Magyar diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 5c652231fd..e9bc609f48 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -25,6 +25,27 @@ #include "geometry_msgs/msg/wrench.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +namespace +{ // utility + +// called from RT control loop +void reset_controller_reference_msg(trajectory_msgs::msg::JointTrajectoryPoint & msg) +{ + msg.positions.clear(); + msg.velocities.clear(); +} + +// called from RT control loop +void reset_wrench_msg( + geometry_msgs::msg::WrenchStamped & msg, + const std::shared_ptr & node) +{ + msg.header.stamp = node->now(); + msg.wrench = geometry_msgs::msg::Wrench(); +} + +} // namespace + namespace admittance_controller { @@ -133,7 +154,6 @@ AdmittanceController::on_export_reference_interfaces() reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); position_reference_ = {}; velocity_reference_ = {}; - input_wrench_command_.reset(); // assign reference interfaces auto index = 0ul; @@ -286,7 +306,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( // setup subscribers and publishers auto joint_command_callback = [this](const std::shared_ptr msg) - { input_joint_command_.writeFromNonRT(msg); }; + { input_joint_command_.set(*msg); }; input_joint_command_subscriber_ = get_node()->create_subscription( "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback); @@ -307,21 +327,20 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( msg.header.frame_id.c_str(), admittance_->parameters_.ft_sensor.frame.id.c_str()); return; } - input_wrench_command_.writeFromNonRT(msg); + input_wrench_command_.set(msg); }); + s_publisher_ = get_node()->create_publisher( "~/status", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique>(s_publisher_); // Initialize state message - state_publisher_->lock(); - state_publisher_->msg_ = admittance_->get_controller_state(); - state_publisher_->unlock(); + state_msg_ = admittance_->get_controller_state(); // Initialize FTS semantic semantic_component force_torque_sensor_ = std::make_unique( - semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); + admittance_->parameters_.ft_sensor.name); // configure admittance rule if ( @@ -390,6 +409,8 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( return controller_interface::CallbackReturn::ERROR; } } + reset_controller_reference_msg(joint_command_msg_); + reset_wrench_msg(wrench_command_msg_, get_node()); // Use current joint_state as a default reference last_reference_ = joint_state_; @@ -408,26 +429,29 @@ controller_interface::return_type AdmittanceController::update_reference_from_su { return controller_interface::return_type::ERROR; } - - joint_command_msg_ = *input_joint_command_.readFromRT(); + auto joint_command_msg_op = input_joint_command_.try_get(); + if (joint_command_msg_op.has_value()) + { + joint_command_msg_ = joint_command_msg_op.value(); + } // if message exists, load values into references - if (joint_command_msg_.get()) + if (!joint_command_msg_.positions.empty() || !joint_command_msg_.velocities.empty()) { for (const auto & interface : admittance_->parameters_.chainable_command_interfaces) { if (interface == hardware_interface::HW_IF_POSITION) { - for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) + for (size_t i = 0; i < joint_command_msg_.positions.size(); ++i) { - position_reference_[i].get() = joint_command_msg_->positions[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) + for (size_t i = 0; i < joint_command_msg_.velocities.size(); ++i) { - velocity_reference_[i].get() = joint_command_msg_->velocities[i]; + velocity_reference_[i].get() = joint_command_msg_.velocities[i]; } } } @@ -451,7 +475,13 @@ controller_interface::return_type AdmittanceController::update_and_write_command // get all controller inputs read_state_from_hardware(joint_state_, ft_values_); - auto offsetted_ft_values = add_wrenches(ft_values_, input_wrench_command_.readFromRT()->wrench); + auto wrench_command_op = input_wrench_command_.try_get(); + if (wrench_command_op.has_value()) + { + wrench_command_msg_ = wrench_command_op.value(); + } + + auto offsetted_ft_values = add_wrenches(ft_values_, wrench_command_msg_.wrench); // apply admittance control to reference to determine desired state admittance_->update(joint_state_, offsetted_ft_values, reference_, period, reference_admittance_); @@ -460,9 +490,11 @@ controller_interface::return_type AdmittanceController::update_and_write_command write_state_to_hardware(reference_admittance_); // Publish controller state - state_publisher_->lock(); - state_publisher_->msg_ = admittance_->get_controller_state(); - state_publisher_->unlockAndPublish(); + if (state_publisher_) + { + state_msg_ = admittance_->get_controller_state(); + state_publisher_->try_publish(state_msg_); + } return controller_interface::return_type::OK; } @@ -498,6 +530,9 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( release_interfaces(); admittance_->reset(num_joints_); + reset_controller_reference_msg(joint_command_msg_); + reset_wrench_msg(wrench_command_msg_, get_node()); + return CallbackReturn::SUCCESS; } @@ -528,21 +563,37 @@ void AdmittanceController::read_state_from_hardware( { if (has_position_state_interface_) { - state_current.positions[joint_ind] = - state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value(); - nan_position |= std::isnan(state_current.positions[joint_ind]); + const auto state_current_position_op = + state_interfaces_[pos_ind * num_joints_ + joint_ind].get_optional(); + nan_position |= + !state_current_position_op.has_value() || std::isnan(state_current_position_op.value()); + if (state_current_position_op.has_value()) + { + state_current.positions[joint_ind] = state_current_position_op.value(); + } } 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]); + auto state_current_velocity_op = + state_interfaces_[vel_ind * num_joints_ + joint_ind].get_optional(); + nan_velocity |= + !state_current_velocity_op.has_value() || std::isnan(state_current_velocity_op.value()); + + if (state_current_velocity_op.has_value()) + { + state_current.velocities[joint_ind] = state_current_velocity_op.value(); + } } if (has_acceleration_state_interface_) { - state_current.accelerations[joint_ind] = - state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); - nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]); + auto state_current_acceleration_op = + state_interfaces_[acc_ind * num_joints_ + joint_ind].get_optional(); + nan_acceleration |= !state_current_acceleration_op.has_value() || + std::isnan(state_current_acceleration_op.value()); + if (state_current_acceleration_op.has_value()) + { + state_current.accelerations[joint_ind] = state_current_acceleration_op.value(); + } } } @@ -578,23 +629,31 @@ void AdmittanceController::write_state_to_hardware( 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_; + + auto logger = get_node()->get_logger(); + for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind) { + bool success = true; if (has_position_command_interface_) { - command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( + success &= command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( state_commanded.positions[joint_ind]); } if (has_velocity_command_interface_) { - command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( + success &= command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( state_commanded.velocities[joint_ind]); } if (has_acceleration_command_interface_) { - command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( + success &= command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( state_commanded.accelerations[joint_ind]); } + if (!success) + { + RCLCPP_WARN(logger, "Error while setting command for joint %zu.", joint_ind); + } } last_commanded_ = state_commanded; } diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 80db4be228..711f4edfd6 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -358,6 +358,31 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) subscribe_and_get_messages(msg); } +TEST_F(AdmittanceControllerTest, check_frame_ids_in_controller_state) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + broadcast_tfs(); // force torque sensor + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + const auto & msg = controller_->admittance_->get_controller_state(); + + // Ensure correct frame IDs + ASSERT_EQ( + msg.ref_trans_base_ft.header.frame_id, controller_->admittance_->parameters_.kinematics.base); + ASSERT_EQ( + msg.ref_trans_base_ft.child_frame_id, controller_->admittance_->parameters_.ft_sensor.frame.id); + ASSERT_EQ( + msg.admittance_position.header.frame_id, controller_->admittance_->parameters_.kinematics.base); + ASSERT_EQ(msg.admittance_position.child_frame_id, "admittance_offset"); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index c2abdecf65..e38bcc82cf 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -67,6 +67,7 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon FRIEND_TEST(AdmittanceControllerTest, check_interfaces); FRIEND_TEST(AdmittanceControllerTest, activate_success); FRIEND_TEST(AdmittanceControllerTest, receive_message_and_publish_updated_status); + FRIEND_TEST(AdmittanceControllerTest, check_frame_ids_in_controller_state); public: CallbackReturn on_init() override diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 255a07c6f4..af510bfcc6 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Rename ackermann msg to controller state msg type (`#1662 `_) diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 0ad9dd193e..dec1544eed 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -40,8 +40,15 @@ 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}) + bicycle_steering_controller_parameters + steering_controllers_library::steering_controllers_library + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${std_srvs_TARGETS}) pluginlib_export_plugin_description_file( controller_interface bicycle_steering_controller.xml) @@ -54,10 +61,10 @@ if(BUILD_TESTING) add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") ament_add_gmock(test_load_bicycle_steering_controller test/test_load_bicycle_steering_controller.cpp) - ament_target_dependencies(test_load_bicycle_steering_controller - controller_manager - hardware_interface - ros2_control_test_assets + target_link_libraries(test_load_bicycle_steering_controller + controller_manager::controller_manager + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets ) add_rostest_with_parameters_gmock( @@ -65,22 +72,12 @@ if(BUILD_TESTING) ${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_preceding test/test_bicycle_steering_controller_preceding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_preceding_params.yaml) target_include_directories(test_bicycle_steering_controller_preceding PRIVATE include) target_link_libraries(test_bicycle_steering_controller_preceding bicycle_steering_controller) - ament_target_dependencies( - test_bicycle_steering_controller_preceding - controller_interface - hardware_interface - ) endif() install( diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 07e72e096f..fea1f1e6ea 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.24.0 + 4.33.1 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Bence Magyar diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index eb10e5ad00..03fbcfa172 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -64,14 +64,27 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) { + auto logger = get_node()->get_logger(); + 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(); + const auto traction_wheel_value_op = state_interfaces_[STATE_TRACTION_WHEEL].get_optional(); + const auto steering_position_op = state_interfaces_[STATE_STEER_AXIS].get_optional(); + + if (!traction_wheel_value_op.has_value() || !steering_position_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve the data from the traction wheel or steering position!"); + return true; + } + + const double traction_wheel_value = traction_wheel_value_op.value(); + const double steering_position = steering_position_op.value(); + if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position)) { if (params_.position_feedback) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 23103f7974..9708ac6d7b 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -89,13 +89,13 @@ TEST_F(BicycleSteeringControllerTest, activate_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)); + auto msg = controller_->input_ref_.get(); + 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) @@ -126,9 +126,9 @@ TEST_F(BicycleSteeringControllerTest, reactivate_success) 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_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -147,23 +147,24 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) 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); + ControllerReferenceMsg msg; + msg.header.stamp = controller_->get_node()->now(); + msg.twist.linear.x = 0.1; + msg.twist.angular.z = 0.2; + controller_->input_ref_.set(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); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45, + COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -190,12 +191,13 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45, + COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -234,7 +236,8 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status ControllerStateMsg msg; subscribe_and_get_messages(msg); - EXPECT_EQ(msg.linear_velocity_command[0], 1.1); + // never received a valid command, linear velocity should have been reset + EXPECT_EQ(msg.linear_velocity_command[0], 0.0); EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(0.1, 0.2); @@ -245,9 +248,10 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45, + COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/chained_filter_controller/CHANGELOG.rst b/chained_filter_controller/CHANGELOG.rst new file mode 100644 index 0000000000..f12ca18ea3 --- /dev/null +++ b/chained_filter_controller/CHANGELOG.rst @@ -0,0 +1,269 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package chained_filter_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- +* Add a generic chained_filter_controller (backport `#1634 `_) (`#1856 `_) +* Contributors: Ankur Bodhe + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- + +4.25.0 (2025-05-17) +------------------- + +4.24.0 (2025-04-27) +------------------- + +4.23.0 (2025-04-10) +------------------- + +4.22.0 (2025-03-17) +------------------- + +4.21.0 (2025-03-01) +------------------- + +4.20.0 (2025-01-29) +------------------- + +4.19.0 (2025-01-13) +------------------- + +4.18.0 (2024-12-19) +------------------- + +4.17.0 (2024-12-07) +------------------- + +4.16.0 (2024-11-08) +------------------- + +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.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/chained_filter_controller/CMakeLists.txt b/chained_filter_controller/CMakeLists.txt new file mode 100644 index 0000000000..81d4bb7ac0 --- /dev/null +++ b/chained_filter_controller/CMakeLists.txt @@ -0,0 +1,102 @@ +cmake_minimum_required(VERSION 3.10) +project(chained_filter_controller) + +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() + +# Dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + filters + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# Generate parameters from YAML +generate_parameter_library( + chained_filter_parameters + src/chained_filter_parameters.yaml +) + +# Library definition +add_library(${PROJECT_NAME} SHARED + src/chained_filter.cpp +) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +# Dependencies +target_link_libraries(${PROJECT_NAME} + PUBLIC + chained_filter_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + filters::filter_chain + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) + +# Export the plugin description +pluginlib_export_plugin_description_file(controller_interface plugin_description.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_chained_filter_controller + test/test_chained_filter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_chained_filter.yaml + ) + target_link_libraries(test_chained_filter_controller + ${PROJECT_NAME} + ) + + add_rostest_with_parameters_gmock(test_multiple_chained_filter_controller + test/test_multiple_chained_filter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_multiple_chained_filter.yaml + ) + target_link_libraries(test_multiple_chained_filter_controller + ${PROJECT_NAME} + ) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_chained_filter_controller test/test_load_chained_filter_controller.cpp) + target_link_libraries(test_load_chained_filter_controller + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) + +endif() + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS ${PROJECT_NAME} + chained_filter_parameters + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Export targets and dependencies +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/chained_filter_controller/doc/userdoc.rst b/chained_filter_controller/doc/userdoc.rst new file mode 100644 index 0000000000..68808fe351 --- /dev/null +++ b/chained_filter_controller/doc/userdoc.rst @@ -0,0 +1,27 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/chained_filter_controller/doc/userdoc.rst + +.. _chained_filter_controller_userdoc: + +Chained Filter Controller +-------------------------------- +This controller wraps around ``filter_chain`` library from the `filters `__ package. It allows to chain multiple filters together, where the output of one filter is the input of the next one. The controller can be used to apply a sequence of filters to a single interface, the same filter chain to multiple interfaces, or different filter chains to different 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. + + +Full list of parameters: + +.. generate_parameter_library_details:: ../src/chained_filter_parameters.yaml + +An example parameter file for this controller can be found in `the test directory `_ for a single interface: + +.. literalinclude:: ../test/config/test_chained_filter.yaml + :language: yaml + +or for `multiple interfaces `_: + +.. literalinclude:: ../test/config/test_multiple_chained_filter.yaml + :language: yaml diff --git a/chained_filter_controller/include/chained_filter_controller/chained_filter.hpp b/chained_filter_controller/include/chained_filter_controller/chained_filter.hpp new file mode 100644 index 0000000000..66ce8f8f5b --- /dev/null +++ b/chained_filter_controller/include/chained_filter_controller/chained_filter.hpp @@ -0,0 +1,63 @@ +// Copyright 2025 ros2_control PMC +// +// 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 CHAINED_FILTER_CONTROLLER__CHAINED_FILTER_HPP_ +#define CHAINED_FILTER_CONTROLLER__CHAINED_FILTER_HPP_ + +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "filters/filter_chain.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "chained_filter_controller/chained_filter_parameters.hpp" + +namespace chained_filter_controller +{ + +class ChainedFilter : public controller_interface::ChainableControllerInterface +{ +public: + controller_interface::CallbackReturn on_init() override; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::vector on_export_state_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + std::shared_ptr param_listener_; + chained_filter::Params params_; + + std::vector>> filters_; + std::vector output_state_values_; +}; +} // namespace chained_filter_controller +#endif // CHAINED_FILTER_CONTROLLER__CHAINED_FILTER_HPP_ diff --git a/chained_filter_controller/package.xml b/chained_filter_controller/package.xml new file mode 100644 index 0000000000..3cc28a7373 --- /dev/null +++ b/chained_filter_controller/package.xml @@ -0,0 +1,43 @@ + + + + chained_filter_controller + 4.33.1 + ros2_controller for configuring filter chains + + 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/ + + Bence Magyar + Christoph Froehlich + ankur-u24 + + ament_cmake + + generate_parameter_library + ros2_control_cmake + + controller_interface + filters + pluginlib + rclcpp + rclcpp_lifecycle + hardware_interface + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/chained_filter_controller/plugin_description.xml b/chained_filter_controller/plugin_description.xml new file mode 100644 index 0000000000..5cae753145 --- /dev/null +++ b/chained_filter_controller/plugin_description.xml @@ -0,0 +1,10 @@ + + + + A chainable ROS 2 controller that applies a sequence of filters to a state interface using the filters package, + and exports the filtered output as a new state interface. + + + diff --git a/chained_filter_controller/src/chained_filter.cpp b/chained_filter_controller/src/chained_filter.cpp new file mode 100644 index 0000000000..d30fb4666d --- /dev/null +++ b/chained_filter_controller/src/chained_filter.cpp @@ -0,0 +1,171 @@ +// Copyright 2025 ros2_control PMC +// +// 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 "chained_filter_controller/chained_filter.hpp" + +#include +#include + +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/version.h" + +namespace chained_filter_controller +{ + +controller_interface::CallbackReturn ChainedFilter::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration ChainedFilter::command_interface_configuration() const +{ + return {controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::InterfaceConfiguration ChainedFilter::state_interface_configuration() const +{ + return {controller_interface::interface_configuration_type::INDIVIDUAL, params_.input_interfaces}; +} + +controller_interface::CallbackReturn ChainedFilter::on_configure(const rclcpp_lifecycle::State &) +{ + try + { + params_ = param_listener_->get_params(); + + if (params_.input_interfaces.size() != params_.output_interfaces.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Input and output interfaces have to have the same size."); + return controller_interface::CallbackReturn::FAILURE; + } + filters_.resize(params_.input_interfaces.size()); + output_state_values_.resize( + params_.output_interfaces.size(), std::numeric_limits::quiet_NaN()); + + // Check if global filter chain configuration is set, if not we use a per-interface + // configuration + bool has_global_filter_setting = true; + if (params_.input_interfaces.size() > 1) + { + if (!get_node()->has_parameter("filter_chain.filter1.name")) + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "filter_chain.filter1.name"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + desc.read_only = true; + get_node()->declare_parameter( + "filter_chain.filter1.name", rclcpp::ParameterValue{""}, desc); + has_global_filter_setting = + !get_node()->get_parameter("filter_chain.filter1.name").as_string().empty(); + } + RCLCPP_INFO_EXPRESSION( + get_node()->get_logger(), has_global_filter_setting, + "Using global filter setting for all interfaces."); + } + + for (size_t i = 0; i < params_.input_interfaces.size(); ++i) + { + filters_[i] = std::make_unique>("double"); + if (!filters_[i]->configure( + has_global_filter_setting ? "filter_chain" + : params_.input_interfaces[i] + ".filter_chain", + get_node()->get_node_logging_interface(), get_node()->get_node_parameters_interface())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to configure filter chain for interfaces %s. Check the parameters for filters " + "setup.", + params_.input_interfaces[i].c_str()); + return controller_interface::CallbackReturn::FAILURE; + } + } + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ChainedFilter::on_activate(const rclcpp_lifecycle::State &) +{ + std::fill( + output_state_values_.begin(), output_state_values_.end(), + std::numeric_limits::quiet_NaN()); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type ChainedFilter::update_and_write_commands( + const rclcpp::Time &, const rclcpp::Duration &) +{ + for (size_t i = 0; i < params_.input_interfaces.size(); ++i) + { + const auto sensor_op = state_interfaces_[i].get_optional(); + if (!sensor_op.has_value()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to read sensor value from state interface '%s'.", + state_interfaces_[0].get_name().c_str()); + continue; + } + if (std::isfinite(sensor_op.value())) + { + filters_[i]->update(sensor_op.value(), output_state_values_[i]); + } + } + + return controller_interface::return_type::OK; +} + +std::vector ChainedFilter::on_export_state_interfaces() +{ + std::vector state_interfaces; + state_interfaces.reserve(params_.output_interfaces.size()); + for (size_t i = 0; i < params_.output_interfaces.size(); ++i) + { + // Split the output interface name by the last "/" if present + const std::string & full_name = params_.output_interfaces.at(i); + auto pos = full_name.rfind('/'); + const std::string iface_name = + (pos != std::string::npos) ? full_name.substr(pos + 1) : full_name; + const std::string iface_prefix = + (pos != std::string::npos) ? "/" + full_name.substr(0, pos) : ""; + + state_interfaces.emplace_back( + get_node()->get_name() + iface_prefix, iface_name, &output_state_values_.at(i)); + } + return state_interfaces; +} + +controller_interface::return_type ChainedFilter::update_reference_from_subscribers( + const rclcpp::Time &, const rclcpp::Duration &) +{ + return controller_interface::return_type::OK; +} + +} // namespace chained_filter_controller + +PLUGINLIB_EXPORT_CLASS( + chained_filter_controller::ChainedFilter, controller_interface::ChainableControllerInterface) diff --git a/chained_filter_controller/src/chained_filter_parameters.yaml b/chained_filter_controller/src/chained_filter_parameters.yaml new file mode 100644 index 0000000000..6aca1fbfcf --- /dev/null +++ b/chained_filter_controller/src/chained_filter_parameters.yaml @@ -0,0 +1,33 @@ +chained_filter: + input_interfaces: + type: string_array + default_value: [] + read_only: true + description: "Name of the input state interface" + validation: { + not_empty<>: [] + } + output_interfaces: + type: string_array + default_value: [] + read_only: true + description: "Name of the output state interface" + validation: { + not_empty<>: [] + } + filter_chain: { + type: none, + description: "Map of parameters that defines a filter chain, containing filterN as key. Valid for all interfaces, overrides .filter_chain if set. + The fields for each filter are: + type: The filter plugin to be loaded + name: Actual name semantically describing the filter, e.g., low_pass_filter + params: And underlying map of parameters needed for a specific filter, refer to the specific filter documentation." + } + __map_input_interfaces: + filter_chain: { + type: none, + description: "Instead of a single configuration, a distinct configuration can be provided for each input interface. The key is the name of the input interface, and the value is a map of parameters that defines a filter chain, containing filterN as key. The fields for each filter are: + type: The filter plugin to be loaded + name: Actual name semantically describing the filter, e.g., low_pass_filter + params: And underlying map of parameters needed for a specific filter, refer to the specific filter documentation." + } diff --git a/chained_filter_controller/test/config/test_chained_filter.yaml b/chained_filter_controller/test/config/test_chained_filter.yaml new file mode 100644 index 0000000000..a2cfe01273 --- /dev/null +++ b/chained_filter_controller/test/config/test_chained_filter.yaml @@ -0,0 +1,10 @@ +test_chained_filter: + ros__parameters: + filter_chain: + filter1: + name: filter1 + type: "filters/MeanFilterDouble" + params: + number_of_observations: 2 + input_interfaces: ["wheel_left/position"] + output_interfaces: ["wheel_left/filtered_position"] diff --git a/chained_filter_controller/test/config/test_multiple_chained_filter.yaml b/chained_filter_controller/test/config/test_multiple_chained_filter.yaml new file mode 100644 index 0000000000..d4fa493b5d --- /dev/null +++ b/chained_filter_controller/test/config/test_multiple_chained_filter.yaml @@ -0,0 +1,37 @@ +test_chained_filter_multiple_interfaces: + ros__parameters: + filter_chain: + filter1: + name: position_filter + type: "filters/MeanFilterDouble" + params: + number_of_observations: 2 + input_interfaces: + - "wheel_left/position" + - "wheel_right/position" + output_interfaces: + - "wheel_left/filtered_position" + - "wheel_right/filtered_position" + +test_chained_filter_multiple_interfaces_config_per_input: + ros__parameters: + input_interfaces: + - "wheel_left/position" + - "wheel_right/position" + output_interfaces: + - "wheel_left/filtered_position" + - "wheel_right/filtered_position" + wheel_left/position: + filter_chain: + filter1: + name: wheel_left_filter + type: "filters/MeanFilterDouble" + params: + number_of_observations: 2 + wheel_right/position: + filter_chain: + filter1: + name: wheel_right_filter + type: "filters/MeanFilterDouble" + params: + number_of_observations: 3 diff --git a/chained_filter_controller/test/test_chained_filter.cpp b/chained_filter_controller/test/test_chained_filter.cpp new file mode 100644 index 0000000000..d1238c00f7 --- /dev/null +++ b/chained_filter_controller/test/test_chained_filter.cpp @@ -0,0 +1,168 @@ +// Copyright 2025 ros2_control PMC +// +// 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_chained_filter.hpp" + +#include + +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "chained_filter_controller/chained_filter.hpp" + +using chained_filter_controller::ChainedFilter; +using controller_interface::CallbackReturn; +using testing::SizeIs; + +using hardware_interface::LoanedStateInterface; + +void ChainedFilterTest::SetUpTestCase() { /*rclcpp::init(0, nullptr);*/ } + +void ChainedFilterTest::TearDownTestCase() { /*rclcpp::shutdown(); */ } + +void ChainedFilterTest::SetUp() +{ + // initialize controller + controller_ = std::make_unique(); +} + +void ChainedFilterTest::TearDown() { controller_.reset(nullptr); } + +void ChainedFilterTest::SetUpController( + const std::string node_name, const std::vector & parameters) +{ + auto node_options = controller_->define_custom_node_options(); + node_options.parameter_overrides(parameters); + + const auto result = controller_->init(node_name, "", 0, "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector state_ifs; + state_ifs.emplace_back(joint_1_pos_); + controller_->assign_interfaces({}, std::move(state_ifs)); + executor.add_node(controller_->get_node()->get_node_base_interface()); +} + +TEST_F(ChainedFilterTest, InitReturnsSuccess) { SetUpController(); } + +TEST_F(ChainedFilterTest, InitFailureWithNoParams) +{ + const auto result = controller_->init( + "test_chained_filter_no_params", "", 0, "", controller_->define_custom_node_options()); + EXPECT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(ChainedFilterTest, ConfigureFailureWithWrongInterfaceSizes) +{ + SetUpController( + "test_chained_filter", + {rclcpp::Parameter("input_interfaces", std::vector{"wheel_left/position"}), + rclcpp::Parameter( + "output_interfaces", + std::vector{"wheel_left/filtered_position", "extra_interface"})}); + auto configure_result = controller_->on_configure(rclcpp_lifecycle::State()); + EXPECT_EQ(configure_result, CallbackReturn::FAILURE); +} + +TEST_F(ChainedFilterTest, ActivateReturnsSuccessWithoutError) +{ + SetUpController(); + + auto configure_result = controller_->on_configure(rclcpp_lifecycle::State()); + EXPECT_EQ(configure_result, CallbackReturn::SUCCESS); + + auto activate_result = controller_->on_activate(rclcpp_lifecycle::State()); + EXPECT_EQ(activate_result, CallbackReturn::SUCCESS); + + ASSERT_FALSE(controller_->is_in_chained_mode()) + << "No controller is claiming the reference interfaces (it has none)."; +} + +TEST_F(ChainedFilterTest, state_interface_configuration_succeeds_when_wheels_are_specified) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(1u)); + 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(0u)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + + auto state_if_exported_conf = controller_->export_state_interfaces(); + ASSERT_THAT(state_if_exported_conf, SizeIs(1u)); + EXPECT_EQ( + state_if_exported_conf[0]->get_name(), "test_chained_filter/wheel_left/filtered_position"); + EXPECT_EQ(state_if_exported_conf[0]->get_interface_name(), "filtered_position"); + EXPECT_EQ(state_if_exported_conf[0]->get_prefix_name(), "test_chained_filter/wheel_left"); +} + +TEST_F(ChainedFilterTest, UpdateFilter) +{ + SetUpController(); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), + controller_interface::return_type::OK); + // input state interface should not change + EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); + // output should be the same + auto state_if_exported_conf = controller_->export_state_interfaces(); + ASSERT_THAT(state_if_exported_conf, SizeIs(1u)); + EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]); + + ASSERT_TRUE(joint_1_pos_.set_value(2.0)); + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), + controller_interface::return_type::OK); + // input and output should have changed + EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); + EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), 1.55); + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), + controller_interface::return_type::OK); + // output should have reached steady state (mean filter) + EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]); +} + +TEST_F(ChainedFilterTest, DeactivateDoesNotCrash) +{ + EXPECT_NO_THROW({ controller_->on_deactivate(rclcpp_lifecycle::State()); }); +} + +TEST_F(ChainedFilterTest, CleanupDoesNotCrash) +{ + EXPECT_NO_THROW({ controller_->on_cleanup(rclcpp_lifecycle::State()); }); +} + +TEST_F(ChainedFilterTest, ShutdownDoesNotCrash) +{ + EXPECT_NO_THROW({ controller_->on_shutdown(rclcpp_lifecycle::State()); }); +} + +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/chained_filter_controller/test/test_chained_filter.hpp b/chained_filter_controller/test/test_chained_filter.hpp new file mode 100644 index 0000000000..8ce63433ac --- /dev/null +++ b/chained_filter_controller/test/test_chained_filter.hpp @@ -0,0 +1,56 @@ +// Copyright 2020 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. + +#ifndef TEST_CHAINED_FILTER_HPP_ +#define TEST_CHAINED_FILTER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "chained_filter_controller/chained_filter.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::HW_IF_POSITION; +using hardware_interface::StateInterface; + +class ChainedFilterTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController( + const std::string node_name = "test_chained_filter", + const std::vector & parameters = {}); + +protected: + std::unique_ptr controller_; + + // dummy joint state values used for tests + const std::vector joint_names_ = {"wheel_left"}; + std::vector joint_states_ = {1.1}; + + StateInterface joint_1_pos_{joint_names_[0], HW_IF_POSITION, &joint_states_[0]}; + rclcpp::executors::SingleThreadedExecutor executor; +}; + +#endif // TEST_CHAINED_FILTER_HPP_ diff --git a/chained_filter_controller/test/test_load_chained_filter_controller.cpp b/chained_filter_controller/test/test_load_chained_filter_controller.cpp new file mode 100644 index 0000000000..886015df35 --- /dev/null +++ b/chained_filter_controller/test/test_load_chained_filter_controller.cpp @@ -0,0 +1,48 @@ +// Copyright 2020 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. + +#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(TestLoadChainedFilter, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::diffbot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/config/test_chained_filter.yaml"; + + cm.set_parameter({"test_chained_filter.params_file", test_file_path}); + cm.set_parameter({"test_chained_filter.type", "chained_filter_controller/ChainedFilter"}); + + ASSERT_NE(cm.load_controller("test_chained_filter"), 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/chained_filter_controller/test/test_multiple_chained_filter.cpp b/chained_filter_controller/test/test_multiple_chained_filter.cpp new file mode 100644 index 0000000000..0ec774a8c4 --- /dev/null +++ b/chained_filter_controller/test/test_multiple_chained_filter.cpp @@ -0,0 +1,202 @@ +// Copyright 2025 ros2_control PMC +// +// 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_multiple_chained_filter.hpp" + +#include + +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "chained_filter_controller/chained_filter.hpp" + +using chained_filter_controller::ChainedFilter; +using controller_interface::CallbackReturn; +using testing::SizeIs; + +using hardware_interface::LoanedStateInterface; + +void MultipleChainedFilterTest::SetUpTestCase() { /*rclcpp::init(0, nullptr);*/ } + +void MultipleChainedFilterTest::TearDownTestCase() { /*rclcpp::shutdown(); */ } + +void MultipleChainedFilterTest::SetUp() +{ + // initialize controller + controller_ = std::make_unique(); +} + +void MultipleChainedFilterTest::TearDown() { controller_.reset(nullptr); } + +void MultipleChainedFilterTest::SetUpController( + const std::string node_name, const std::vector & parameters) +{ + auto node_options = controller_->define_custom_node_options(); + node_options.parameter_overrides(parameters); + + const auto result = controller_->init(node_name, "", 0, "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector state_ifs; + state_ifs.emplace_back(joint_1_pos_); + state_ifs.emplace_back(joint_2_pos_); + controller_->assign_interfaces({}, std::move(state_ifs)); + executor.add_node(controller_->get_node()->get_node_base_interface()); +} + +TEST_F(MultipleChainedFilterTest, InitReturnsSuccess) { SetUpController(); } + +TEST_F(MultipleChainedFilterTest, ActivateReturnsSuccessWithoutError) +{ + SetUpController(); + + auto configure_result = controller_->on_configure(rclcpp_lifecycle::State()); + EXPECT_EQ(configure_result, CallbackReturn::SUCCESS); + + auto activate_result = controller_->on_activate(rclcpp_lifecycle::State()); + EXPECT_EQ(activate_result, CallbackReturn::SUCCESS); + + ASSERT_FALSE(controller_->is_in_chained_mode()) + << "No controller is claiming the reference interfaces (it has none)."; +} + +TEST_F( + MultipleChainedFilterTest, + state_interface_configuration_succeeds_when_multiple_wheels_are_specified) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2u)); + 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(0u)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + + auto state_if_exported_conf = controller_->export_state_interfaces(); + ASSERT_THAT(state_if_exported_conf, SizeIs(2u)); + EXPECT_EQ( + state_if_exported_conf[0]->get_name(), + "test_chained_filter_multiple_interfaces/wheel_left/filtered_position"); + EXPECT_EQ( + state_if_exported_conf[0]->get_prefix_name(), + "test_chained_filter_multiple_interfaces/wheel_left"); + EXPECT_EQ(state_if_exported_conf[0]->get_interface_name(), "filtered_position"); + + EXPECT_EQ( + state_if_exported_conf[1]->get_name(), + "test_chained_filter_multiple_interfaces/wheel_right/filtered_position"); + EXPECT_EQ( + state_if_exported_conf[1]->get_prefix_name(), + "test_chained_filter_multiple_interfaces/wheel_right"); + EXPECT_EQ(state_if_exported_conf[1]->get_interface_name(), "filtered_position"); +} + +TEST_F(MultipleChainedFilterTest, UpdateFilter_multiple_interfaces) +{ + SetUpController(); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), + controller_interface::return_type::OK); + // input state interface should not change + EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); + EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]); + // output should be the same + auto state_if_exported_conf = controller_->export_state_interfaces(); + ASSERT_THAT(state_if_exported_conf, SizeIs(2u)); + EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]); + EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), joint_states_[1]); + + ASSERT_TRUE(joint_1_pos_.set_value(2.0)); + ASSERT_TRUE(joint_2_pos_.set_value(3.0)); + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), + controller_interface::return_type::OK); + // input and output should have changed + EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); + EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), 1.55); + EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]); + EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), 2.6); + + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), + controller_interface::return_type::OK); + // output should have reached steady state (mean filter) + EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]); + EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), joint_states_[1]); +} + +TEST_F(MultipleChainedFilterTest, UpdateFilter_multiple_interfaces_config_per_input) +{ + SetUpController("test_chained_filter_multiple_interfaces_config_per_input"); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), + controller_interface::return_type::OK); + // input state interface should not change + EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); + EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]); + // output should be the same + auto state_if_exported_conf = controller_->export_state_interfaces(); + ASSERT_THAT(state_if_exported_conf, SizeIs(2u)); + EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]); + EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), joint_states_[1]); + + ASSERT_TRUE(joint_1_pos_.set_value(2.0)); + ASSERT_TRUE(joint_2_pos_.set_value(2.8)); + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), + controller_interface::return_type::OK); + // input and output should have changed + EXPECT_EQ(joint_1_pos_.get_optional().value(), joint_states_[0]); + EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), 1.55); + EXPECT_EQ(joint_2_pos_.get_optional().value(), joint_states_[1]); + // second update call, mean of (2.2, 2.8) + EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), 2.5); + + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), + controller_interface::return_type::OK); + // output should have reached steady state for the first input (mean filter) + EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]); + // third update call, mean of (2.2, 2.8, 2.8) + EXPECT_EQ(state_if_exported_conf[1]->get_optional().value(), 2.6); + + ASSERT_EQ( + controller_->update_and_write_commands(rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)), + controller_interface::return_type::OK); + // no change + EXPECT_EQ(state_if_exported_conf[0]->get_optional().value(), joint_states_[0]); + // output should have reached steady state (mean filter) + EXPECT_NEAR(state_if_exported_conf[1]->get_optional().value(), joint_states_[1], 1e-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/chained_filter_controller/test/test_multiple_chained_filter.hpp b/chained_filter_controller/test/test_multiple_chained_filter.hpp new file mode 100644 index 0000000000..d2be05a7d9 --- /dev/null +++ b/chained_filter_controller/test/test_multiple_chained_filter.hpp @@ -0,0 +1,57 @@ +// Copyright 2020 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. + +#ifndef TEST_MULTIPLE_CHAINED_FILTER_HPP_ +#define TEST_MULTIPLE_CHAINED_FILTER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "chained_filter_controller/chained_filter.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::HW_IF_POSITION; +using hardware_interface::StateInterface; + +class MultipleChainedFilterTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController( + const std::string node_name = "test_chained_filter_multiple_interfaces", + const std::vector & parameters = {}); + +protected: + std::unique_ptr controller_; + + // dummy joint state values used for tests + const std::vector joint_names_ = {"wheel_left", "wheel_right"}; + std::vector joint_states_ = {1.1, 2.2}; + + StateInterface joint_1_pos_{joint_names_[0], HW_IF_POSITION, &joint_states_[0]}; + StateInterface joint_2_pos_{joint_names_[1], HW_IF_POSITION, &joint_states_[1]}; + rclcpp::executors::SingleThreadedExecutor executor; +}; + +#endif // TEST_MULTIPLE_CHAINED_FILTER_HPP_ diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 50347022c1..6af36a154e 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,58 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- +* Don't use `msg\_` field of realtime publisher (backport `#1947 `_) (`#1948 `_) +* Contributors: mergify[bot] + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Fix: Remove deprecated `rclcpp::spin_some(node)` (backport `#1928 `_) (`#1932 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- +* Explicit cast rcutils_duration_value_t (backport `#1808 `_) (`#1812 `_) +* Update description of limit() function in speed_limiter (backport `#1793 `_) (`#1795 `_) +* Contributors: Christoph Fröhlich, mergify[bot] + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Fix DiffDrive claiming state when open_loop is set (`#1730 `_) +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: Jasper van Brakel, mergify[bot] + +4.25.0 (2025-05-17) +------------------- +* Simplify `on_set_chained_mode` avoiding cpplint warnings (backport `#1564 `_) (`#1688 `_) +* Deprecating tf2 C Headers (`#1325 `_) +* Contributors: Lucas Wendland, mergify[bot], Bhagyesh Agresar + 4.24.0 (2025-04-27) ------------------- * Call `configure()` of base class instead of node (`#1659 `_) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 2acbf8b69e..7d57720a26 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -48,8 +48,19 @@ target_include_directories(diff_drive_controller PUBLIC target_link_libraries(diff_drive_controller PUBLIC diff_drive_controller_parameters - control_toolbox::rate_limiter_parameters) -ament_target_dependencies(diff_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + control_toolbox::rate_limiter_parameters + control_toolbox::control_toolbox + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + rcpputils::rcpputils + realtime_tools::realtime_tools + tf2::tf2 + ${tf2_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS}) pluginlib_export_plugin_description_file(controller_interface diff_drive_plugin.xml) if(BUILD_TESTING) @@ -63,22 +74,12 @@ if(BUILD_TESTING) target_link_libraries(test_diff_drive_controller diff_drive_controller ) - ament_target_dependencies(test_diff_drive_controller - geometry_msgs - hardware_interface - nav_msgs - rclcpp - rclcpp_lifecycle - realtime_tools - tf2 - tf2_msgs - ) add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") ament_add_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp) - ament_target_dependencies(test_load_diff_drive_controller - controller_manager - ros2_control_test_assets + target_link_libraries(test_load_diff_drive_controller + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) endif() diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 2fce07a31b..f2ec74f7d9 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -41,6 +41,7 @@ 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. +Unless the parameter ``open_loop=true`` is set, then no external state interfaces are used (the commanded velocity is used for odometry instead). Output ,,,,,,,,, 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 665203c1c9..fca6e30039 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 @@ -32,8 +32,8 @@ #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "tf2_msgs/msg/tf_message.hpp" // auto-generated by generate_parameter_library @@ -83,7 +83,7 @@ class DiffDriveController : public controller_interface::ChainableControllerInte struct WheelHandle { - std::reference_wrapper feedback; + std::optional> feedback; std::reference_wrapper velocity; }; @@ -113,16 +113,21 @@ class DiffDriveController : public controller_interface::ChainableControllerInte std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> realtime_odometry_publisher_ = nullptr; + nav_msgs::msg::Odometry odometry_message_; std::shared_ptr> odometry_transform_publisher_ = nullptr; std::shared_ptr> realtime_odometry_transform_publisher_ = nullptr; + tf2_msgs::msg::TFMessage odometry_transform_message_; bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> received_velocity_msg_ptr_{nullptr}; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox received_velocity_msg_; + // save the last reference in case of unable to get value from box + TwistStamped command_msg_; std::queue> previous_two_commands_; // speed limiters @@ -133,6 +138,7 @@ class DiffDriveController : public controller_interface::ChainableControllerInte std::shared_ptr> limited_velocity_publisher_ = nullptr; std::shared_ptr> realtime_limited_velocity_publisher_ = nullptr; + TwistStamped limited_velocity_message_; rclcpp::Time previous_update_timestamp_{0}; diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp index 937e848460..8e13a35ad1 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -96,7 +96,7 @@ class SpeedLimiter } /** - * \brief Limit the velocity and acceleration + * \brief Limit the velocity, acceleration, and jerk * \param [in, out] v Velocity [m/s] * \param [in] v0 Previous velocity to v [m/s] * \param [in] v1 Previous velocity to v0 [m/s] diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index dded814eb9..f6f74d0f2b 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.24.0 + 4.33.1 Controller for a differential-drive mobile base. Bence Magyar diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index c3554dfecb..f119774984 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -85,6 +85,11 @@ InterfaceConfiguration DiffDriveController::command_interface_configuration() co InterfaceConfiguration DiffDriveController::state_interface_configuration() const { + if (params_.open_loop) + { + return {interface_configuration_type::NONE, {}}; + } + std::vector conf_names; for (const auto & joint_name : params_.left_wheel_names) { @@ -102,15 +107,13 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub { auto logger = get_node()->get_logger(); - const std::shared_ptr command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT()); - - if (command_msg_ptr == nullptr) + auto current_ref_op = received_velocity_msg_.try_get(); + if (current_ref_op.has_value()) { - RCLCPP_WARN(logger, "Velocity message received was a nullptr."); - return controller_interface::return_type::ERROR; + command_msg_ = current_ref_op.value(); } - const auto age_of_last_command = time - command_msg_ptr->header.stamp; + const auto age_of_last_command = time - command_msg_.header.stamp; // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { @@ -118,16 +121,16 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub reference_interfaces_[1] = 0.0; } else if ( - std::isfinite(command_msg_ptr->twist.linear.x) && - std::isfinite(command_msg_ptr->twist.angular.z)) + std::isfinite(command_msg_.twist.linear.x) && std::isfinite(command_msg_.twist.angular.z)) { - reference_interfaces_[0] = command_msg_ptr->twist.linear.x; - reference_interfaces_[1] = command_msg_ptr->twist.angular.z; + reference_interfaces_[0] = command_msg_.twist.linear.x; + reference_interfaces_[1] = command_msg_.twist.angular.z; } else { RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger, *get_node()->get_clock(), cmd_vel_timeout_.seconds() * 1000, + logger, *get_node()->get_clock(), + static_cast(cmd_vel_timeout_.seconds() * 1000), "Command message contains NaNs. Not updating reference interfaces."); } @@ -168,9 +171,9 @@ controller_interface::return_type DiffDriveController::update_and_write_commands for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { const auto left_feedback_op = - registered_left_wheel_handles_[index].feedback.get().get_optional(); + registered_left_wheel_handles_[index].feedback.value().get().get_optional(); const auto right_feedback_op = - registered_right_wheel_handles_[index].feedback.get().get_optional(); + registered_right_wheel_handles_[index].feedback.value().get().get_optional(); if (!left_feedback_op.has_value() || !right_feedback_op.has_value()) { @@ -228,24 +231,23 @@ controller_interface::return_type DiffDriveController::update_and_write_commands if (should_publish) { - if (realtime_odometry_publisher_->trylock()) + if (realtime_odometry_publisher_) { - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.stamp = time; - odometry_message.pose.pose.position.x = odometry_.getX(); - odometry_message.pose.pose.position.y = odometry_.getY(); - odometry_message.pose.pose.orientation.x = orientation.x(); - odometry_message.pose.pose.orientation.y = orientation.y(); - odometry_message.pose.pose.orientation.z = orientation.z(); - odometry_message.pose.pose.orientation.w = orientation.w(); - odometry_message.twist.twist.linear.x = odometry_.getLinear(); - odometry_message.twist.twist.angular.z = odometry_.getAngular(); - realtime_odometry_publisher_->unlockAndPublish(); + odometry_message_.header.stamp = time; + odometry_message_.pose.pose.position.x = odometry_.getX(); + odometry_message_.pose.pose.position.y = odometry_.getY(); + odometry_message_.pose.pose.orientation.x = orientation.x(); + odometry_message_.pose.pose.orientation.y = orientation.y(); + odometry_message_.pose.pose.orientation.z = orientation.z(); + odometry_message_.pose.pose.orientation.w = orientation.w(); + odometry_message_.twist.twist.linear.x = odometry_.getLinear(); + odometry_message_.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->try_publish(odometry_message_); } - if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_) { - auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + auto & transform = odometry_transform_message_.transforms.front(); transform.header.stamp = time; transform.transform.translation.x = odometry_.getX(); transform.transform.translation.y = odometry_.getY(); @@ -253,7 +255,7 @@ controller_interface::return_type DiffDriveController::update_and_write_commands transform.transform.rotation.y = orientation.y(); transform.transform.rotation.z = orientation.z(); transform.transform.rotation.w = orientation.w(); - realtime_odometry_transform_publisher_->unlockAndPublish(); + realtime_odometry_transform_publisher_->try_publish(odometry_transform_message_); } } @@ -268,17 +270,16 @@ controller_interface::return_type DiffDriveController::update_and_write_commands previous_two_commands_.push({{linear_command, angular_command}}); // Publish limited velocity - if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) + if (publish_limited_velocity_ && realtime_limited_velocity_publisher_) { - auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; - limited_velocity_command.header.stamp = time; - limited_velocity_command.twist.linear.x = linear_command; - limited_velocity_command.twist.linear.y = 0.0; - limited_velocity_command.twist.linear.z = 0.0; - limited_velocity_command.twist.angular.x = 0.0; - limited_velocity_command.twist.angular.y = 0.0; - limited_velocity_command.twist.angular.z = angular_command; - realtime_limited_velocity_publisher_->unlockAndPublish(); + limited_velocity_message_.header.stamp = time; + limited_velocity_message_.twist.linear.x = linear_command; + limited_velocity_message_.twist.linear.y = 0.0; + limited_velocity_message_.twist.linear.z = 0.0; + limited_velocity_message_.twist.angular.x = 0.0; + limited_velocity_message_.twist.angular.y = 0.0; + limited_velocity_message_.twist.angular.z = angular_command; + realtime_limited_velocity_publisher_->try_publish(limited_velocity_message_); } // Compute wheels velocities: @@ -452,7 +453,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( cmd_vel_timeout_ == rclcpp::Duration::from_seconds(0.0) || current_time_diff < cmd_vel_timeout_) { - received_velocity_msg_ptr_.writeFromNonRT(msg); + received_velocity_msg_.set(*msg); } else { @@ -499,16 +500,15 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( 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 = odom_frame_id; - odometry_message.child_frame_id = 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_ = params_.publish_rate; publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); // initialize odom values zeros - odometry_message.twist = + odometry_message_.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); constexpr size_t NUM_DIMENSIONS = 6; @@ -516,8 +516,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { // 0, 7, 14, 21, 28, 35 const size_t diagonal_index = NUM_DIMENSIONS * index + index; - odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; - odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + odometry_message_.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_message_.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } // initialize transform publisher and message @@ -528,10 +528,9 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_transform_publisher_); // keeping track of odom and base_link transforms only - auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; - odometry_transform_message.transforms.resize(1); - odometry_transform_message.transforms.front().header.frame_id = odom_frame_id; - odometry_transform_message.transforms.front().child_frame_id = base_frame_id; + odometry_transform_message_.transforms.resize(1); + odometry_transform_message_.transforms.front().header.frame_id = odom_frame_id; + odometry_transform_message_.transforms.front().child_frame_id = base_frame_id; previous_update_timestamp_ = get_node()->get_clock()->now(); return controller_interface::CallbackReturn::SUCCESS; @@ -623,27 +622,29 @@ void DiffDriveController::reset_buffers() previous_two_commands_.push({{0.0, 0.0}}); previous_two_commands_.push({{0.0, 0.0}}); - // Fill RealtimeBuffer with NaNs so it will contain a known value + // Fill RealtimeBox with NaNs so it will contain a known value // but still indicate that no command has yet been sent. - received_velocity_msg_ptr_.reset(); - std::shared_ptr empty_msg_ptr = std::make_shared(); - empty_msg_ptr->header.stamp = get_node()->now(); - empty_msg_ptr->twist.linear.x = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.linear.y = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.linear.z = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.angular.x = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.angular.y = std::numeric_limits::quiet_NaN(); - empty_msg_ptr->twist.angular.z = std::numeric_limits::quiet_NaN(); - received_velocity_msg_ptr_.writeFromNonRT(empty_msg_ptr); + command_msg_.header.stamp = get_node()->now(); + command_msg_.twist.linear.x = std::numeric_limits::quiet_NaN(); + command_msg_.twist.linear.y = std::numeric_limits::quiet_NaN(); + command_msg_.twist.linear.z = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.x = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.y = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.z = std::numeric_limits::quiet_NaN(); + received_velocity_msg_.set(command_msg_); } void DiffDriveController::halt() { - const auto halt_wheels = [](auto & wheel_handles) + auto logger = get_node()->get_logger(); + const auto halt_wheels = [&](auto & wheel_handles) { for (const auto & wheel_handle : wheel_handles) { - wheel_handle.velocity.get().set_value(0.0); + if (!wheel_handle.velocity.get().set_value(0.0)) + { + RCLCPP_WARN(logger, "Failed to set wheel velocity to value 0.0"); + } } }; @@ -667,21 +668,6 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( registered_handles.reserve(wheel_names.size()); for (const auto & wheel_name : wheel_names) { - const auto interface_name = feedback_type(); - const auto state_handle = std::find_if( - state_interfaces_.cbegin(), state_interfaces_.cend(), - [&wheel_name, &interface_name](const auto & interface) - { - return interface.get_prefix_name() == wheel_name && - interface.get_interface_name() == interface_name; - }); - - if (state_handle == state_interfaces_.cend()) - { - RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); - return controller_interface::CallbackReturn::ERROR; - } - const auto command_handle = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), [&wheel_name](const auto & interface) @@ -696,18 +682,36 @@ controller_interface::CallbackReturn DiffDriveController::configure_side( return controller_interface::CallbackReturn::ERROR; } - registered_handles.emplace_back( - WheelHandle{std::ref(*state_handle), std::ref(*command_handle)}); + if (params_.open_loop) + { + registered_handles.emplace_back(WheelHandle{std::nullopt, std::ref(*command_handle)}); + } + else + { + const auto interface_name = feedback_type(); + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&wheel_name, &interface_name](const auto & interface) + { + return interface.get_prefix_name() == wheel_name && + interface.get_interface_name() == interface_name; + }); + + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + registered_handles.emplace_back( + WheelHandle{{std::ref(*state_handle)}, std::ref(*command_handle)}); + } } return controller_interface::CallbackReturn::SUCCESS; } -bool DiffDriveController::on_set_chained_mode(bool chained_mode) -{ - // Always accept switch to/from chained mode (without linting type-cast error) - return true || chained_mode; -} +bool DiffDriveController::on_set_chained_mode(bool /*chained_mode*/) { return true; } std::vector DiffDriveController::on_export_reference_interfaces() diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 1b01e5b067..17cea9e7a7 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -45,10 +45,6 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr { public: using DiffDriveController::DiffDriveController; - std::shared_ptr getLastReceivedTwist() - { - return *(received_velocity_msg_ptr_.readFromNonRT()); - } /** * @brief wait_for_twist block until a new twist is received. @@ -77,7 +73,13 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr { return realtime_odometry_publisher_; } - + // Declare these tests as friends so we can access odometry_message_ + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace); + FRIEND_TEST(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace); // Declare these tests as friends so we can access controller_->reference_interfaces_ FRIEND_TEST(TestDiffDriveController, chainable_controller_unchained_mode); FRIEND_TEST(TestDiffDriveController, chainable_controller_chained_mode); @@ -134,7 +136,7 @@ class TestDiffDriveController : public ::testing::Test } /// \brief wait for the subscriber and publisher to completely setup - void waitForSetup() + void waitForSetup(rclcpp::Executor & executor) { constexpr std::chrono::seconds TIMEOUT{2}; auto clock = pub_node->get_clock(); @@ -145,7 +147,8 @@ class TestDiffDriveController : public ::testing::Test { FAIL(); } - rclcpp::spin_some(pub_node); + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } } @@ -175,6 +178,15 @@ class TestDiffDriveController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + void assignResourcesNoFeedback() + { + std::vector command_ifs; + command_ifs.emplace_back(left_wheel_vel_cmd_); + command_ifs.emplace_back(right_wheel_vel_cmd_); + + controller_->assign_interfaces(std::move(command_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, @@ -262,6 +274,26 @@ TEST_F( EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } +TEST_F( + TestDiffDriveController, + command_and_state_interface_configuration_succeeds_when_wheels_and_open_loop_are_specified) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("open_loop", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(0)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); + 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); +} + TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { std::string odom_id = "odom"; @@ -279,12 +311,9 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names 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); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, base_link_id); } TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) @@ -304,14 +333,10 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp 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 */ - ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); - ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, frame_prefix + "/" + base_link_id); } TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) @@ -331,13 +356,10 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_names 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 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); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, base_link_id); } TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace) @@ -360,12 +382,9 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name 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); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, base_link_id); } TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace) @@ -388,14 +407,10 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names 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); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, frame_prefix + "/" + base_link_id); } TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) @@ -417,14 +432,11 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name 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); + ASSERT_EQ(controller_->odometry_message_.header.frame_id, ns_prefix + odom_id); + ASSERT_EQ(controller_->odometry_message_.child_frame_id, ns_prefix + base_link_id); } TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) @@ -458,6 +470,19 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } +TEST_F(TestDiffDriveController, activate_succeeds_with_open_loop_assigned) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("open_loop", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResourcesNoFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + TEST_F(TestDiffDriveController, test_speed_limiter) { // If you send a linear velocity command without acceleration limits, @@ -494,7 +519,7 @@ TEST_F(TestDiffDriveController, test_speed_limiter) state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - waitForSetup(); + waitForSetup(executor); // send msg publish(0.0, 0.0); @@ -690,6 +715,34 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } +TEST_F(TestDiffDriveController, activate_silently_ignores_with_unnecessary_resources_assigned_1) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("open_loop", rclcpp::ParameterValue(true)), + rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResourcesPosFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestDiffDriveController, activate_silently_ignores_with_unnecessary_resources_assigned_2) +{ + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("open_loop", rclcpp::ParameterValue(true)), + rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResourcesVelFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + TEST_F(TestDiffDriveController, cleanup) { ASSERT_EQ( @@ -707,7 +760,7 @@ TEST_F(TestDiffDriveController, cleanup) state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - waitForSetup(); + waitForSetup(executor); // send msg const double linear = 1.0; @@ -828,7 +881,7 @@ TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - waitForSetup(); + waitForSetup(executor); // Reference interfaces should be NaN on initialization // (Note: reference_interfaces_ is protected, but this is @@ -924,7 +977,7 @@ TEST_F(TestDiffDriveController, chainable_controller_chained_mode) state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - waitForSetup(); + waitForSetup(executor); // Reference interfaces should be NaN on initialization for (const auto & interface : controller_->reference_interfaces_) @@ -1028,7 +1081,7 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - waitForSetup(); + waitForSetup(executor); // Reference interfaces should be NaN on initialization // (Note: reference_interfaces_ is protected, but this is @@ -1072,7 +1125,7 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - waitForSetup(); + waitForSetup(executor); // (Note: reference_interfaces_ is protected, but this is // a FRIEND_TEST so we can use it) @@ -1126,7 +1179,7 @@ TEST_F(TestDiffDriveController, command_with_zero_timestamp_is_accepted_with_war state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - waitForSetup(); + waitForSetup(executor); // published command message with zero timestamp sets the command interfaces to the correct values const double linear = 1.0; diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 6fd3b159b2..befde85a3a 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -29,6 +29,7 @@ Controllers for Wheeled Mobile Robots Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> Mecanum Drive Controllers <../mecanum_drive_controller/doc/userdoc.rst> + Omni Wheel Drive Controller <../omni_wheel_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> @@ -76,6 +77,16 @@ In the sense of ros2_control, broadcasters are still controllers using the same Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst> GPS Sensor Broadcaster <../gps_sensor_broadcaster/doc/userdoc.rst> +Filters +********************** + +Chainable controllers for filtering of state interfaces. They export the filtered values as state interfaces, which can be used by other controllers or broadcasters, and don't publish to ROS topics. + +.. toctree:: + :titlesonly: + + Chained Filter Controller <../chained_filter_controller/doc/userdoc.rst> + Common Controller Parameters **************************** diff --git a/doc/images/swerve_drive.drawio b/doc/images/swerve_drive.drawio new file mode 100644 index 0000000000..7f79cad6a5 --- /dev/null +++ b/doc/images/swerve_drive.drawio @@ -0,0 +1,664 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/swerve_drive.svg b/doc/images/swerve_drive.svg new file mode 100644 index 0000000000..7afefd3c18 --- /dev/null +++ b/doc/images/swerve_drive.svg @@ -0,0 +1,4 @@ + + + +
0
1
2
3
l
0
1
2
3
Rotating Clockwise
w
0
1
2
3
Moving Forward
%3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bcolor%3A%20light-dark(rgb(255%2C%20255%2C%20255)%2C%20rgb(255%2C%20255%2C%20255))%3B%26quot%3B%26gt%3B0%26lt%3B%2Ffont%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%2282%22%20y%3D%22185%22%20width%3D%2230%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E%3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bcolor%3A%20light-dark(rgb(255%2C%20255%2C%20255)%2C%20rgb(255%2C%20255%2C%20255))%3B%26quot%3B%26gt%3B0%26lt%3B%2Ffont%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%2282%22%20y%3D%22185%22%20width%3D%2230%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E
0
1
2
3
Moving Right
0
1
2
3
Moving Left
 \ No newline at end of file diff --git a/doc/migration.rst b/doc/migration.rst index 34c4eaa419..349bf110ce 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -1,9 +1,12 @@ :github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration.rst -Migration Guides: Iron to Jazzy +Migration Guides: Humble to Jazzy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This list summarizes important changes between Iron (previous) and Jazzy (current) releases, where changes to user code might be necessary. +This list summarizes important changes between Humble (previous) and Jazzy (current) releases, where changes to user code might be necessary. +gripper_action_controller +***************************** +The ``effort_controllers/GripperActionController`` and ``position_controllers/GripperActionController`` will be removed. The ``parallel_gripper_action_controller/GripperActionController`` should be used instead. `(#1652 `__). diff_drive_controller ***************************** @@ -12,7 +15,6 @@ diff_drive_controller joint_trajectory_controller ***************************** - * Parameter ``allow_nonzero_velocity_at_trajectory_end`` is now per default ``false`` (`#834 `_). * The parameter ``start_with_holding`` is removed, it now always holds the position at activation (`#839 `_). * Goals are now cancelled in ``on_deactivate`` transition (`#962 `_). @@ -22,6 +24,13 @@ 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. * Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 `_). +pid_controller +************************ +* Parameters ``enable_feedforward`` and service ``set_feedforward_control`` are deprecated. Instead, set the feedforward_gain to zero or a non-zero value. (`#1553 `_). +* The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have + been deprecated in favor of the new ``antiwindup_strategy`` parameter (`#1585 `__). Choose a suitable anti-windup strategy and set the parameters accordingly. +* PID state publisher topic changed to ```` namespace and is initially turned off. It can be turned on by using ``activate_state_publisher`` parameter. (`#1823 `_). + steering_controllers_library ******************************** * ``front_steering`` parameter was removed (`#1166 `_). Now every kinematics type (bicycle, tricycle, Ackermann) has dedicated parameters for steering or traction wheels instead of front/rear wheels. diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst index b861733ee7..1b2566ed9a 100644 --- a/doc/mobile_robot_kinematics.rst +++ b/doc/mobile_robot_kinematics.rst @@ -46,7 +46,7 @@ It follows the coordinate conventions defined in `ROS REP 103 `__. + +.. image:: images/swerve_drive.svg + :align: center + :alt: Swerve Drive Robot + +* :math:`x_b, y_b` is the robot's body-frame coordinate system, located at the geometric center of the robot. +* :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:`l` is the wheelbase (distance between front and rear wheels). +* :math:`w` is the track width (distance between left and right wheels). +* Red arrows on wheel :math:`i` signify the direction of the wheel's velocity :math:`v_i`. + +Each swerve module :math:`i`, for :math:`i = 0, 1, 2, 3` (typically front-left, front-right, back-left, back-right) is located at :math:`(l_{i,x}, l_{i,y})` relative to the center, typically: + +* Front-left: :math:`(l/2, w/2)` +* Front-right: :math:`(l/2, -w/2)` +* Back-left: :math:`(-l/2, w/2)` +* Back-right: :math:`(-l/2, -w/2)` + +**Inverse Kinematics** + +For each module :math:`i` at position :math:`(l_{i,x}, l_{i,y})`, the velocity vector is: + +.. math:: + + \begin{bmatrix} + v_{i,x} \\ + v_{i,y} + \end{bmatrix} + = + \begin{bmatrix} + v_{b,x} - \omega_{b,z} l_{i,y} \\ + v_{b,y} + \omega_{b,z} l_{i,x} + \end{bmatrix} + +The wheel velocity :math:`v_i` and steering angle :math:`\phi_i` are: + +.. math:: + + v_i = \sqrt{v_{i,x}^2 + v_{i,y}^2} + +.. math:: + + \phi_i = \arctan2(v_{i,y}, v_{i,x}) + + +**Odometry** + +The body twist of the robot is computed from the wheel velocities :math:`v_i` and steering angles :math:`\phi_i`. Each module’s velocity components in the body frame are: + +.. math:: + + v_{i,x} = v_i \cos(\phi_i), \quad v_{i,y} = v_i \sin(\phi_i) + +The chassis velocities are calculated as: + +.. math:: + + v_{b,x} = \frac{1}{4} \sum_{i=0}^{3} v_{i,x}, \quad v_{b,y} = \frac{1}{4} \sum_{i=0}^{3} v_{i,y} + +.. math:: + + \omega_{b,z} = \frac{\sum_{i=0}^{3} (v_{i,y} l_{i,x} - v_{i,x} l_{i,y})}{\sum_{i=0}^{3} (l_{i,x}^2 + l_{i,y}^2)} + + + +Odometry updates the robot’s pose (:math:`x`, :math:`y`, :math:`\theta`) in the global frame using the computed chassis velocities. The global velocities are: + +.. math:: + + v_{x,\text{global}} = v_{b,x} \cos(\theta) - v_{b,y} \sin(\theta) + +.. math:: + + v_{y,\text{global}} = v_{b,x} \sin(\theta) + v_{b,y} \cos(\theta) + + Nonholonomic Wheeled Mobile Robots ..................................... @@ -338,8 +423,8 @@ The turning radius of the robot is 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) + \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** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index d407917374..325e4f7b35 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -1,8 +1,8 @@ :github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes.rst -Release Notes: Iron to Jazzy +Release Notes: Humble to Jazzy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This list summarizes the changes between Iron (previous) and Jazzy (current) releases. +This list summarizes the changes between Humble (previous) and Jazzy (current) releases. admittance_controller ************************ @@ -16,6 +16,14 @@ diff_drive_controller * Parameters ``has_velocity_limits``, ``has_acceleration_limits``, and ``has_jerk_limits`` are removed. Instead, set the respective limits to ``.NAN``. (`#1315 `_). * Parameters ``max_acceleration_reverse`` and ``max_deceleration_reverse`` were added to configure asymmetric acceleration/deceleration behavior. (`#1315 `_). +gripper_action_controller +***************************** +The ``effort_controllers/GripperActionController`` and ``position_controllers/GripperActionController`` will be removed. The ``parallel_gripper_action_controller/GripperActionController`` should be used instead. `(#1652 `__). + +chained_filter_controller +******************************* +* The chained_filter_controller was added to use generic filter plugins (`#1634 `__). + joint_trajectory_controller ***************************** @@ -54,15 +62,29 @@ joint_trajectory_controller * 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 `_). * Feed-forward effort trajectories are supported now (`#1200 `_). * Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 `_). +* The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior. (`#1759 `__). mecanum_drive_controller ************************ * 🚀 The mecanum_drive_controller was added 🎉 (`#512 `_). +parallel_gripper_action_controller +********************************** +The ``parallel_gripper_action_controller/GripperActionController`` was added. `(#1652 `__). + +omni_wheel_drive_controller +********************************* +* 🚀 The omni_wheel_drive_controller was added 🎉 (`#1535 `_). + pid_controller ************************ * 🚀 The PID controller was added 🎉 (`#434 `_). * Add ``save_i_term`` parameter to control retention of integral state after re-activation (`#1507 `_). +* The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior (`#1585 `__). + * Output clamping via ``u_clamp_max`` and ``u_clamp_min`` was added, allowing users to bound the controller output. + * The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter. A ``tracking_time_constant`` parameter has also been introduced to configure the back-calculation strategy. + * A new ``error_deadband`` parameter stops integration when the error is within a specified range. +* PID state publisher can be turned off or on by using ``activate_state_publisher`` parameter. (`#1823 `_). steering_controllers_library ******************************** @@ -80,3 +102,8 @@ tricycle_controller gpio_controllers ************************ * The GPIO command controller was added 🎉 (`#1251 `_). + +force_torque_sensor_broadcaster +******************************* +* Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 `__). +* Added support for filter chains, allowing users to configure a sequence of filter plugins with their parameters. The force/torque sensor readings are filtered sequentially and published on a separate topic. diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 0874448461..84ebbd2124 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -12,9 +12,8 @@ The following is a step-by-step guide to create source files, basic tests, and c If the package for the controller does not exist, then create it first. The package should have ``ament_cmake`` as a build type. - The easiest way is to search online for the most recent manual. - A helpful command to support this process is ``ros2 pkg create``. - Use the ``--help`` flag for more information on how to use it. + Generally, you can use ``ros2 pkg create --build-type ament_cmake``. + Use the ``--help`` flag for more information on how to use this command. There is also an option to create library source files and compile rules to help you in the following steps. 2. **Preparing source files** @@ -25,17 +24,32 @@ 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. ROS 2-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. 2. include ``"controller_interface/controller_interface.hpp"``. 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., + 4. Define the class of the controller, extending ``ControllerInterface``. Now, your code should look similar to this: .. code:: c++ + #ifndef _HPP_ + #define _HPP_ + + #include "controller_interface/controller_interface.hpp" + + namespace + { + class ControllerName : public controller_interface::ControllerInterface + { + // ... + }; + + } // namespace + + #endif // _HPP_ 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 `_. @@ -53,7 +67,7 @@ The following is a step-by-step guide to create source files, basic tests, and c This could also be done in the ``on_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. + 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::CallbackReturn::SUCCESS`` or ``controller_interface::CallbackReturn::ERROR`` otherwise. 4. Write the ``on_configure`` method. Parameters are usually read here, and everything is prepared so that the controller can be started. @@ -65,7 +79,6 @@ The following is a step-by-step guide to create source files, basic tests, and c 6. Implement the ``on_activate`` method with checking, and potentially sorting, the interfaces and assigning members' initial values. This method is part of the real-time loop, therefore avoid any reservation of memory and, in general, keep it as short as possible. - 7. Implement the ``on_deactivate`` method, which does the opposite of ``on_activate``. In many cases, this method is empty. This method should also be real-time safe as much as possible. @@ -129,10 +142,10 @@ The following is a step-by-step guide to create source files, basic tests, and c 9. **Compiling and testing the controller** - 1. Now everything is ready to compile the controller using the ``colcon build `` command. + 1. Now everything is ready to compile the controller using the ``colcon build --packages-select `` command. Remember to go into the root of your workspace before executing this command. - 2. If compilation was successful, source the ``setup.bash`` file from the install folder and execute ``colcon test `` to check if the new controller can be found through ``pluginlib`` library and be loaded by the controller manager. + 2. If compilation was successful, source the ``setup.bash`` file from the install folder and execute ``colcon test --packages-select `` to check if the new controller can be found through ``pluginlib`` library and be loaded by the controller manager. That's it! Enjoy writing great controllers! diff --git a/downstream.humble.repos b/downstream.humble.repos deleted file mode 100644 index 81feb3723b..0000000000 --- a/downstream.humble.repos +++ /dev/null @@ -1,5 +0,0 @@ -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 deleted file mode 100644 index e5a26574b7..0000000000 --- a/downstream.jazzy.repos +++ /dev/null @@ -1,5 +0,0 @@ -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 deleted file mode 100644 index e5a26574b7..0000000000 --- a/downstream.rolling.repos +++ /dev/null @@ -1,5 +0,0 @@ -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 62da945091..5f1cc053ec 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Call `configure()` of base class instead of node (`#1659 `_) diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index 32b975ffca..be047b1321 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -25,7 +25,11 @@ target_include_directories(effort_controllers PUBLIC $ $ ) -ament_target_dependencies(effort_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(effort_controllers PUBLIC + forward_command_controller::forward_command_controller + pluginlib::pluginlib + rclcpp::rclcpp +) pluginlib_export_plugin_description_file(controller_interface effort_controllers_plugins.xml) @@ -39,10 +43,8 @@ if(BUILD_TESTING) ) 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 + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) ament_add_gmock(test_joint_group_effort_controller diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 35ae9741b2..259215234e 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.24.0 + 4.33.1 Generic controller for forwarding commands. Bence Magyar diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index e7d0f274a5..1d053f4c7f 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -55,11 +55,14 @@ controller_interface::CallbackReturn JointGroupEffortController::on_deactivate( const rclcpp_lifecycle::State & previous_state) { auto ret = ForwardCommandController::on_deactivate(previous_state); - // stop all joints for (auto & command_interface : command_interfaces_) { - command_interface.set_value(0.0); + if (!command_interface.set_value(0.0)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Unable to set command interface value to 0.0"); + return controller_interface::CallbackReturn::SUCCESS; + } } return ret; diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 01125ac442..5fa4394250 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -106,14 +106,14 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -121,9 +121,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); } TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) @@ -133,9 +133,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ( @@ -143,9 +143,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) @@ -160,9 +160,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) @@ -171,9 +171,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -204,9 +204,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); } TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) @@ -218,15 +218,15 @@ TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 0.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 0.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 0.0); } diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d52c436cc4..bccc723531 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,56 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- +* Don't use `msg\_` field of realtime publisher (backport `#1947 `_) (`#1948 `_) +* Contributors: mergify[bot] + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Add filtering capability to ft_broadcaster (backport `#1814 `_) (`#1927 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- +* FTS: Don't make a temporary copy of semantic component (backport `#1902 `_) (`#1904 `_) +* Contributors: mergify[bot] + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- +* Revert temporary logging changes added for CI timeout investigation (backport `#1741 `_) (`#1747 `_) +* Contributors: Julia Jia + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- +* Add multiplier support to ForceTorqueSensorBroadcaster (backport `#1647 `_) (`#1686 `_) +* Contributors: mergify[bot], edward.ix + 4.24.0 (2025-04-27) ------------------- * [CI] test_force_torque_sensor_broadcaster regularily times out (`#1639 `_) diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 3e3786a9f1..9224a17001 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -7,6 +7,7 @@ export_windows_symbols() set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface + filters generate_parameter_library geometry_msgs hardware_interface @@ -35,9 +36,15 @@ target_include_directories(force_torque_sensor_broadcaster PUBLIC $ ) 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}) + force_torque_sensor_broadcaster_parameters + controller_interface::controller_interface + filters::filter_chain + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${geometry_msgs_TARGETS}) pluginlib_export_plugin_description_file( controller_interface force_torque_sensor_broadcaster.xml) @@ -53,11 +60,9 @@ if(BUILD_TESTING) 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 + controller_manager::controller_manager + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets ) add_rostest_with_parameters_gmock(test_force_torque_sensor_broadcaster @@ -67,9 +72,31 @@ if(BUILD_TESTING) target_link_libraries(test_force_torque_sensor_broadcaster force_torque_sensor_broadcaster ) - ament_target_dependencies(test_force_torque_sensor_broadcaster - hardware_interface + + add_library(dummy_filter SHARED + test/dummy_filter.cpp + ) + target_compile_features(dummy_filter PUBLIC cxx_std_17) + target_include_directories(dummy_filter PUBLIC + $ + $ ) + target_link_libraries(dummy_filter PUBLIC + filters::increment + ${geometry_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + ) + install( + TARGETS + dummy_filter + EXPORT export_force_torque_sensor_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + ) + + pluginlib_export_plugin_description_file(filters "test/dummy_filter_plugin.xml") endif() install( diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index df0430e3bb..bfa82d77f6 100644 --- a/force_torque_sensor_broadcaster/doc/userdoc.rst +++ b/force_torque_sensor_broadcaster/doc/userdoc.rst @@ -7,8 +7,11 @@ Force Torque Sensor Broadcaster Broadcaster of messages from force/torque state interfaces of a robot or sensor. The published message type is ``geometry_msgs/msg/WrenchStamped``. -The controller is a wrapper around ``ForceTorqueSensor`` semantic component (see ``controller_interface`` package). +The broadcaster also supports filtering the force/torque readings using a filter chain, enabling the sequential application of multiple filters. In this case, an additional topic with the +``_filtered`` suffix will be published containing the final result. For more details on filters, refer to the `filters package repository `_. +See the parameter section for instructions on configuring a filter chain with an arbitrary number of filters. +The controller is a wrapper around ``ForceTorqueSensor`` semantic component (see ``controller_interface`` package). Parameters ^^^^^^^^^^^ @@ -17,6 +20,12 @@ This controller uses the `generate_parameter_library `_ library to load each filter at runtime, +passing the specified parameters. + +The chain processes the data sequentially, passing the output of one filter as the input to the next. + Full list of parameters: .. generate_parameter_library_details:: ../src/force_torque_sensor_broadcaster_parameters.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 34b7f242ee..1f0b4484b7 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 @@ -20,18 +20,23 @@ #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #include +#include #include #include "controller_interface/chainable_controller_interface.hpp" -// auto-generated by generate_parameter_library -#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster_parameters.hpp" +#include "filters/filter_chain.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" +// auto-generated by generate_parameter_library +#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster_parameters.hpp" + namespace force_torque_sensor_broadcaster { +using WrenchMsgType = geometry_msgs::msg::WrenchStamped; + class ForceTorqueSensorBroadcaster : public controller_interface::ChainableControllerInterface { public: @@ -61,15 +66,24 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ChainableContr protected: void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg); + void apply_sensor_multiplier(const Params & params, geometry_msgs::msg::WrenchStamped & msg); std::shared_ptr param_listener_; Params params_; std::unique_ptr force_torque_sensor_; - - using StatePublisher = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr sensor_state_publisher_; - std::unique_ptr realtime_publisher_; + bool has_filter_chain_ = false; + + WrenchMsgType wrench_raw_; + WrenchMsgType wrench_filtered_; + std::unique_ptr> filter_chain_; + + using StatePublisher = rclcpp::Publisher::SharedPtr; + using StateRTPublisher = realtime_tools::RealtimePublisher; + StatePublisher sensor_raw_state_publisher_; + StatePublisher sensor_filtered_state_publisher_; + std::unique_ptr realtime_raw_publisher_; + std::unique_ptr realtime_filtered_publisher_; }; } // namespace force_torque_sensor_broadcaster diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 8c98f892ff..96c6c84c3c 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.24.0 + 4.33.1 Controller to publish state of force-torque sensors. Bence Magyar @@ -25,6 +25,7 @@ backward_ros controller_interface + filters geometry_msgs hardware_interface pluginlib 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 ef5e74e89b..bb99606bdd 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -18,9 +18,12 @@ #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" +#include #include #include +#include "rclcpp/logging.hpp" + namespace force_torque_sensor_broadcaster { ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() @@ -74,39 +77,53 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( if (!params_.sensor_name.empty()) { - force_torque_sensor_ = std::make_unique( - semantic_components::ForceTorqueSensor(params_.sensor_name)); - - // TODO(juliaj): remove the logging after resolving - // https://github.com/ros-controls/ros2_controllers/issues/1574 - RCLCPP_INFO( - get_node()->get_logger(), "Initialized force_torque_sensor with sensor name %s", - params_.sensor_name.c_str()); + force_torque_sensor_ = + std::make_unique(params_.sensor_name); } else { auto const & force_names = params_.interface_names.force; auto const & torque_names = params_.interface_names.torque; force_torque_sensor_ = std::make_unique( - semantic_components::ForceTorqueSensor( - force_names.x, force_names.y, force_names.z, torque_names.x, torque_names.y, - torque_names.z)); + force_names.x, force_names.y, force_names.z, torque_names.x, torque_names.y, torque_names.z); + } - // TODO(juliaj): remove the logging after resolving - // https://github.com/ros-controls/ros2_controllers/issues/1574 - RCLCPP_INFO( - get_node()->get_logger(), - "Initialized force_torque_sensor with interface names %s, %s, %s, %s, %s, %s", - force_names.x.c_str(), force_names.y.c_str(), force_names.z.c_str(), torque_names.x.c_str(), - torque_names.y.c_str(), torque_names.z.c_str()); + try + { + filter_chain_ = + std::make_unique>("geometry_msgs::msg::WrenchStamped"); } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during filter chain creation with message : %s \n", e.what()); + return CallbackReturn::ERROR; + } + + // As the sensor_filter_chain parameter is of type 'none', we cannot directly check if it is + // present. Even if the sensor_filter_chain parameter is not specified, the filter chain will be + // correctly configured with an empty list of filters. + bool filter_chain_configured = filter_chain_->configure( + "sensor_filter_chain", get_node()->get_node_logging_interface(), + get_node()->get_node_parameters_interface()); + + // Even on successful configure, if empty, the chain won't be used + has_filter_chain_ = filter_chain_configured; + + RCLCPP_INFO_EXPRESSION( + get_node()->get_logger(), has_filter_chain_, "Filter chain configured successfully!"); try { // register ft sensor data publisher - sensor_state_publisher_ = get_node()->create_publisher( + sensor_raw_state_publisher_ = get_node()->create_publisher( "~/wrench", rclcpp::SystemDefaultsQoS()); - realtime_publisher_ = std::make_unique(sensor_state_publisher_); + realtime_raw_publisher_ = std::make_unique(sensor_raw_state_publisher_); + + sensor_filtered_state_publisher_ = + get_node()->create_publisher( + "~/wrench_filtered", rclcpp::SystemDefaultsQoS()); + realtime_filtered_publisher_ = + std::make_unique(sensor_filtered_state_publisher_); } catch (const std::exception & e) { @@ -116,19 +133,10 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } - // TODO(juliaj): remove the logging after resolving - // https://github.com/ros-controls/ros2_controllers/issues/1574 - RCLCPP_INFO(get_node()->get_logger(), "Locking realtime publisher"); - realtime_publisher_->lock(); - RCLCPP_INFO(get_node()->get_logger(), "Locked realtime publisher"); - - realtime_publisher_->msg_.header.frame_id = params_.frame_id; - - RCLCPP_INFO(get_node()->get_logger(), "Unlocking realtime publisher"); - realtime_publisher_->unlock(); - RCLCPP_INFO(get_node()->get_logger(), "Unlocked realtime publisher"); + wrench_raw_.header.frame_id = params_.frame_id; + wrench_filtered_.header.frame_id = params_.frame_id; - RCLCPP_INFO(get_node()->get_logger(), "Configure successful"); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -170,12 +178,26 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write { params_ = param_listener_->get_params(); } - if (realtime_publisher_ && realtime_publisher_->trylock()) + + wrench_raw_.header.stamp = time; + force_torque_sensor_->get_values_as_message(wrench_raw_.wrench); + this->apply_sensor_offset(params_, wrench_raw_); + this->apply_sensor_multiplier(params_, wrench_raw_); + + if (realtime_raw_publisher_) + { + realtime_raw_publisher_->try_publish(wrench_raw_); + } + + if (has_filter_chain_) { - realtime_publisher_->msg_.header.stamp = time; - force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); - this->apply_sensor_offset(params_, realtime_publisher_->msg_); - realtime_publisher_->unlockAndPublish(); + // Filter sensor data, if no filter chain config was specified, wrench_filtered_ = wrench_raw_ + auto filtered = filter_chain_->update(wrench_raw_, wrench_filtered_); + if (filtered && realtime_filtered_publisher_) + { + wrench_filtered_.header.stamp = time; + realtime_filtered_publisher_->try_publish(wrench_filtered_); + } } return controller_interface::return_type::OK; @@ -221,38 +243,32 @@ ForceTorqueSensorBroadcaster::on_export_state_interfaces() if (!force_names[0].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, force_names[0], &realtime_publisher_->msg_.wrench.force.x)); + export_prefix, force_names[0], &wrench_raw_.wrench.force.x); } if (!force_names[1].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, force_names[1], &realtime_publisher_->msg_.wrench.force.y)); + export_prefix, force_names[1], &wrench_raw_.wrench.force.y); } if (!force_names[2].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, force_names[2], &realtime_publisher_->msg_.wrench.force.z)); + export_prefix, force_names[2], &wrench_raw_.wrench.force.z); } if (!torque_names[0].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x)); + export_prefix, torque_names[0], &wrench_raw_.wrench.torque.x); } if (!torque_names[1].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y)); + export_prefix, torque_names[1], &wrench_raw_.wrench.torque.y); } if (!torque_names[2].empty()) { exported_state_interfaces.emplace_back( - hardware_interface::StateInterface( - export_prefix, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z)); + export_prefix, torque_names[2], &wrench_raw_.wrench.torque.z); } return exported_state_interfaces; } @@ -267,6 +283,17 @@ void ForceTorqueSensorBroadcaster::apply_sensor_offset( msg.wrench.torque.y += params.offset.torque.y; msg.wrench.torque.z += params.offset.torque.z; } + +void ForceTorqueSensorBroadcaster::apply_sensor_multiplier( + const Params & params, geometry_msgs::msg::WrenchStamped & msg) +{ + msg.wrench.force.x *= params.multiplier.force.x; + msg.wrench.force.y *= params.multiplier.force.y; + msg.wrench.force.z *= params.multiplier.force.z; + msg.wrench.torque.x *= params.multiplier.torque.x; + msg.wrench.torque.y *= params.multiplier.torque.y; + msg.wrench.torque.z *= params.multiplier.torque.z; +} } // namespace force_torque_sensor_broadcaster #include "pluginlib/class_list_macros.hpp" 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 0869f5cf3c..fe1c7cedbb 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 @@ -79,3 +79,43 @@ force_torque_sensor_broadcaster: default_value: 0.0, description: "The offset of torque values around 'z' axis.", } + multiplier: + force: + x: { + type: double, + default_value: 1.0, + description: "The multiplier of force value on 'x' axis.", + } + y: { + type: double, + default_value: 1.0, + description: "The multiplier of force value on 'y' axis.", + } + z: { + type: double, + default_value: 1.0, + description: "The multiplier of force value on 'z' axis.", + } + torque: + x: { + type: double, + default_value: 1.0, + description: "The multiplier of torque value around 'x' axis.", + } + y: { + type: double, + default_value: 1.0, + description: "The multiplier of torque value around 'y' axis.", + } + z: { + type: double, + default_value: 1.0, + description: "The multiplier of torque value around 'z' axis.", + } + sensor_filter_chain: { + type: none, + description: "Map of parameters that defines a filter chain, containing filterN as key. The fields for each filter are: + type: The filter plugin to be loaded + name: Actual name semantically describing the filter, e.g., low_pass_filter + params: And underlying map of parameters needed for a specific filter, refer to the specific filter documentation." + } diff --git a/force_torque_sensor_broadcaster/test/dummy_filter.cpp b/force_torque_sensor_broadcaster/test/dummy_filter.cpp new file mode 100644 index 0000000000..3eef7fb3df --- /dev/null +++ b/force_torque_sensor_broadcaster/test/dummy_filter.cpp @@ -0,0 +1,56 @@ + +// Copyright (c) 2025, PAL 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. + +/* + * Authors: Oscar Martinez + */ + +#include +#include + +#include "geometry_msgs/msg/wrench_stamped.hpp" + +#include "filters/increment.hpp" + +namespace filters +{ + +template <> +bool IncrementFilter::update( + const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) +{ + if (!this->configured_) + { + throw std::runtime_error("Filter is not configured"); + } + + // Just increment every value + data_out.wrench.force.x = data_in.wrench.force.x + 1; + data_out.wrench.force.y = data_in.wrench.force.y + 1; + data_out.wrench.force.z = data_in.wrench.force.z + 1; + data_out.wrench.torque.x = data_in.wrench.torque.x + 1; + data_out.wrench.torque.y = data_in.wrench.torque.y + 1; + data_out.wrench.torque.z = data_in.wrench.torque.z + 1; + + return true; +} + +} // namespace filters + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + filters::IncrementFilter, + filters::FilterBase) diff --git a/force_torque_sensor_broadcaster/test/dummy_filter_plugin.xml b/force_torque_sensor_broadcaster/test/dummy_filter_plugin.xml new file mode 100644 index 0000000000..17623c6e44 --- /dev/null +++ b/force_torque_sensor_broadcaster/test/dummy_filter_plugin.xml @@ -0,0 +1,9 @@ + + + + + This is a increment filter which works on a wrench message. + + + + 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 index 58cc1c6b16..1c09892ccb 100644 --- a/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml +++ b/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml @@ -1,4 +1,11 @@ test_force_torque_sensor_broadcaster: ros__parameters: - frame_id: "fts_sensor_frame" +test_force_torque_sensor_broadcaster_with_chain: + ros__parameters: + frame_id: "fts_sensor_frame" + sensor_name: "fts_sensor" + sensor_filter_chain: + filter1: + name: dummy + type: filters/IncrementFilterWrench 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 f6781aab04..017928888f 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 @@ -19,6 +19,7 @@ #include "test_force_torque_sensor_broadcaster.hpp" #include +#include #include #include @@ -28,6 +29,7 @@ #include "rclcpp/executor.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" + using hardware_interface::LoanedStateInterface; using testing::IsEmpty; using testing::SizeIs; @@ -48,19 +50,12 @@ void ForceTorqueSensorBroadcasterTest::SetUp() fts_broadcaster_ = std::make_unique(); } -void ForceTorqueSensorBroadcasterTest::TearDown() -{ - // TODO(juliaj): remove the logging after resolving - // https://github.com/ros-controls/ros2_controllers/issues/1574 - RCLCPP_INFO(fts_broadcaster_->get_node()->get_logger(), "In TearDown"); - fts_broadcaster_.reset(nullptr); -} +void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullptr); } -void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() +void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster(std::string node_name) { - const auto result = fts_broadcaster_->init( - "test_force_torque_sensor_broadcaster", "", 0, "", - fts_broadcaster_->define_custom_node_options()); + const auto result = + fts_broadcaster_->init(node_name, "", 0, "", fts_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; @@ -72,13 +67,10 @@ void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() state_ifs.emplace_back(fts_torque_z_); fts_broadcaster_->assign_interfaces({}, std::move(state_ifs)); - // TODO(juliaj): remove the logging after resolving - // https://github.com/ros-controls/ros2_controllers/issues/1574 - RCLCPP_INFO(fts_broadcaster_->get_node()->get_logger(), "FTSBroadcaster setup done"); } void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( - geometry_msgs::msg::WrenchStamped & wrench_msg) + geometry_msgs::msg::WrenchStamped & wrench_msg, std::string & topic_name) { // create a new subscriber geometry_msgs::msg::WrenchStamped::SharedPtr received_msg; @@ -86,7 +78,7 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( 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); + topic_name, 10, subs_callback); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(test_subscription_node.get_node_base_interface()); @@ -119,7 +111,7 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // configure failed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -127,7 +119,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the 'sensor_name' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); @@ -142,7 +134,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSet) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the 'sensor_name' empty fts_broadcaster_->get_node()->set_parameter({"sensor_name", ""}); @@ -153,7 +145,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSe TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSet) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the 'interface_names' empty fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", ""}); @@ -165,7 +157,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSe TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the 'sensor_name' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); @@ -187,11 +179,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) { - SetUpFTSBroadcaster(); - - // TODO(juliaj): remove the logging after resolving - // https://github.com/ros-controls/ros2_controllers/issues/1574 - RCLCPP_INFO(fts_broadcaster_->get_node()->get_logger(), "Setting up interface names"); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the 'interface_names' fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); @@ -208,7 +196,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the params 'sensor_name' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); @@ -240,7 +228,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the params 'sensor_name' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); @@ -256,7 +244,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the params 'interface_names' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); @@ -272,7 +260,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the params 'sensor_name' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); @@ -282,7 +270,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); geometry_msgs::msg::WrenchStamped wrench_msg; - subscribe_and_get_message(wrench_msg); + std::string topic_name = "/test_force_torque_sensor_broadcaster/wrench"; + subscribe_and_get_message(wrench_msg, topic_name); ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0]); @@ -295,7 +284,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); std::array force_offsets = {{10.0, 30.0, -50.0}}; std::array torque_offsets = {{1.0, -1.2, -5.2}}; @@ -313,7 +302,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); geometry_msgs::msg::WrenchStamped wrench_msg; - subscribe_and_get_message(wrench_msg); + std::string topic_name = "/test_force_torque_sensor_broadcaster/wrench"; + subscribe_and_get_message(wrench_msg, topic_name); ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] + force_offsets[0]); @@ -350,14 +340,85 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets ASSERT_EQ( exported_state_interfaces[i]->get_prefix_name(), controller_name + "/" + sensor_name_); ASSERT_EQ( - exported_state_interfaces[i]->get_value(), + exported_state_interfaces[i]->get_optional().value(), sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); } } +TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Multipliers) +{ + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); + + // some non‐trivial multipliers + std::array force_multipliers = {{2.0, 0.5, -1.0}}; + std::array torque_multipliers = {{-2.0, 3.0, 0.0}}; + + // Set the required params + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Set all multiplier parameters + fts_broadcaster_->get_node()->set_parameter({"multiplier.force.x", force_multipliers[0]}); + fts_broadcaster_->get_node()->set_parameter({"multiplier.force.y", force_multipliers[1]}); + fts_broadcaster_->get_node()->set_parameter({"multiplier.force.z", force_multipliers[2]}); + fts_broadcaster_->get_node()->set_parameter({"multiplier.torque.x", torque_multipliers[0]}); + fts_broadcaster_->get_node()->set_parameter({"multiplier.torque.y", torque_multipliers[1]}); + fts_broadcaster_->get_node()->set_parameter({"multiplier.torque.z", torque_multipliers[2]}); + + // Configure & activate + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Publish & grab the message + geometry_msgs::msg::WrenchStamped wrench_msg; + std::string topic_name = "/test_force_torque_sensor_broadcaster/wrench"; + subscribe_and_get_message(wrench_msg, topic_name); + + // Check header + ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); + + // Check that each field was scaled accordingly + ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] * force_multipliers[0]); + ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1] * force_multipliers[1]); + ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2] * force_multipliers[2]); + ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] * torque_multipliers[0]); + ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] * torque_multipliers[1]); + ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] * torque_multipliers[2]); + + // the exported state interfaces reflect the same scaled values + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 6u); + + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ( + exported_state_interfaces[0]->get_name(), controller_name + "/" + sensor_name_ + "/force.x"); + ASSERT_EQ( + exported_state_interfaces[1]->get_name(), controller_name + "/" + sensor_name_ + "/force.y"); + ASSERT_EQ( + exported_state_interfaces[2]->get_name(), controller_name + "/" + sensor_name_ + "/force.z"); + ASSERT_EQ( + exported_state_interfaces[3]->get_name(), controller_name + "/" + sensor_name_ + "/torque.x"); + ASSERT_EQ( + exported_state_interfaces[4]->get_name(), controller_name + "/" + sensor_name_ + "/torque.y"); + ASSERT_EQ( + exported_state_interfaces[5]->get_name(), controller_name + "/" + sensor_name_ + "/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "force.y"); + ASSERT_EQ(exported_state_interfaces[2]->get_interface_name(), "force.z"); + ASSERT_EQ(exported_state_interfaces[3]->get_interface_name(), "torque.x"); + ASSERT_EQ(exported_state_interfaces[4]->get_interface_name(), "torque.y"); + ASSERT_EQ(exported_state_interfaces[5]->get_interface_name(), "torque.z"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ( + exported_state_interfaces[i]->get_optional().value(), + sensor_values_[i] * (i < 3 ? force_multipliers[i] : torque_multipliers[i - 3])); + } +} + TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set the params 'interface_names' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); @@ -368,7 +429,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); geometry_msgs::msg::WrenchStamped wrench_msg; - subscribe_and_get_message(wrench_msg); + std::string topic_name = "/test_force_torque_sensor_broadcaster/wrench"; + subscribe_and_get_message(wrench_msg, topic_name); ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0]); @@ -388,13 +450,13 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) ASSERT_EQ(exported_state_interfaces[1]->get_prefix_name(), controller_name); ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "fts_sensor/force.x"); ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "fts_sensor/torque.z"); - ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]); - ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]); + ASSERT_EQ(exported_state_interfaces[0]->get_optional().value(), sensor_values_[0]); + ASSERT_EQ(exported_state_interfaces[1]->get_optional().value(), sensor_values_[5]); } TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) { - SetUpFTSBroadcaster(); + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster"); // set all the params 'interface_names' and 'frame_id' fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); @@ -409,7 +471,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); geometry_msgs::msg::WrenchStamped wrench_msg; - subscribe_and_get_message(wrench_msg); + std::string topic_name = "/test_force_torque_sensor_broadcaster/wrench"; + subscribe_and_get_message(wrench_msg, topic_name); ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0]); @@ -438,10 +501,113 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) for (size_t i = 0; i < 6; ++i) { ASSERT_EQ(exported_state_interfaces[0]->get_prefix_name(), controller_name); - ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]); + ASSERT_EQ(exported_state_interfaces[i]->get_optional().value(), sensor_values_[i]); } } +TEST_F(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Configure_Success) +{ + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster_with_chain"); + + // configure passed + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->has_filter_chain_, true); + + // 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); + + std::cout << "Finished" << std::endl; +} + +TEST_F(ForceTorqueSensorBroadcasterTest, SensorFilterChain_ActivateDeactivate_Success) +{ + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster_with_chain"); + + // configure and activate success + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->has_filter_chain_, true); + 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, SensorFilterChain_Update_Success) +{ + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster_with_chain"); + + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->has_filter_chain_, true); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // Dummy wrenches + geometry_msgs::msg::WrenchStamped wrench_in; + geometry_msgs::msg::WrenchStamped wrench_out; + + wrench_in.wrench.force.x = 1.0; + wrench_in.wrench.force.y = 2.0; + wrench_in.wrench.force.z = 3.0; + wrench_in.wrench.torque.x = 4.0; + wrench_in.wrench.torque.y = 5.0; + wrench_in.wrench.torque.z = 6.0; + + // Update the filter chain directly (the values from the update are just published) + fts_broadcaster_->filter_chain_->update(wrench_in, wrench_out); + + ASSERT_EQ(wrench_out.wrench.force.x, wrench_in.wrench.force.x + 1.0); + ASSERT_EQ(wrench_out.wrench.force.y, wrench_in.wrench.force.y + 1.0); + ASSERT_EQ(wrench_out.wrench.force.z, wrench_in.wrench.force.z + 1.0); + ASSERT_EQ(wrench_out.wrench.torque.x, wrench_in.wrench.torque.x + 1.0); + ASSERT_EQ(wrench_out.wrench.torque.y, wrench_in.wrench.torque.y + 1.0); + ASSERT_EQ(wrench_out.wrench.torque.z, wrench_in.wrench.torque.z + 1.0); +} + +TEST_F(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Publish_Success) +{ + SetUpFTSBroadcaster("test_force_torque_sensor_broadcaster_with_chain"); + + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->has_filter_chain_, true); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + geometry_msgs::msg::WrenchStamped wrench_msg_filtered; + std::string topic_name = "/test_force_torque_sensor_broadcaster_with_chain/wrench_filtered"; + subscribe_and_get_message(wrench_msg_filtered, topic_name); + + ASSERT_EQ(wrench_msg_filtered.header.frame_id, frame_id_); + ASSERT_EQ(wrench_msg_filtered.wrench.force.x, sensor_values_[0] + 1.0); + ASSERT_EQ(wrench_msg_filtered.wrench.force.y, sensor_values_[1] + 1.0); + ASSERT_EQ(wrench_msg_filtered.wrench.force.z, sensor_values_[2] + 1.0); + ASSERT_EQ(wrench_msg_filtered.wrench.torque.x, sensor_values_[3] + 1.0); + ASSERT_EQ(wrench_msg_filtered.wrench.torque.y, sensor_values_[4] + 1.0); + ASSERT_EQ(wrench_msg_filtered.wrench.torque.z, sensor_values_[5] + 1.0); +} + int main(int argc, char ** argv) { ::testing::InitGoogleMock(&argc, argv); 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 5d485d9d12..5bd6b62452 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 @@ -39,6 +39,11 @@ class FriendForceTorqueSensorBroadcaster FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, UpdateTest); FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorStatePublishTest); + + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Configure_Success); + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorFilterChain_ActivateDeactivate_Success); + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Update_Success); + FRIEND_TEST(ForceTorqueSensorBroadcasterTest, SensorFilterChain_Publish_Success); }; class ForceTorqueSensorBroadcasterTest : public ::testing::Test @@ -50,7 +55,7 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test void SetUp(); void TearDown(); - void SetUpFTSBroadcaster(); + void SetUpFTSBroadcaster(std::string node_name); protected: const std::string sensor_name_ = "fts_sensor"; @@ -66,7 +71,8 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test std::unique_ptr fts_broadcaster_; - void subscribe_and_get_message(geometry_msgs::msg::WrenchStamped & wrench_msg); + void subscribe_and_get_message( + geometry_msgs::msg::WrenchStamped & wrench_msg, std::string & topic_name); }; #endif // TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 39c997c65f..57e7cb7a3d 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,49 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- +* Reject non-finite values in forward controller subscriber callback (#… (backport `#1815 `_) (`#1817 `_) +* Contributors: Christoph Fröhlich, Kaizen + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Call `configure()` of base class instead of node (`#1659 `_) diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 06888549c4..542fa45537 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -42,10 +42,15 @@ 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}) + forward_command_controller_parameters + multi_interface_forward_command_controller_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${std_msgs_TARGETS}) pluginlib_export_plugin_description_file(controller_interface forward_command_plugin.xml) if(BUILD_TESTING) @@ -58,11 +63,8 @@ if(BUILD_TESTING) ) 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 + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) ament_add_gmock(test_forward_command_controller @@ -77,11 +79,8 @@ if(BUILD_TESTING) ) 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 + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) ament_add_gmock(test_multi_interface_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 c8b620d593..9f03042cdf 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 @@ -22,7 +22,7 @@ #include "controller_interface/controller_interface.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "std_msgs/msg/float64_multi_array.hpp" namespace forward_command_controller @@ -86,7 +86,11 @@ class ForwardControllersBase : public controller_interface::ControllerInterface std::vector command_interface_types_; - realtime_tools::RealtimeBuffer> rt_command_ptr_; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox rt_command_; + // save the last reference in case of unable to get value from box + CmdType joint_commands_; + rclcpp::Subscription::SharedPtr joints_command_subscriber_; }; diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 097d8865a0..336efa774c 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.24.0 + 4.33.1 Generic controller for forwarding commands. Bence Magyar diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index 331d3f9eea..b83e22a298 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -14,6 +14,8 @@ #include "forward_command_controller/forward_controllers_base.hpp" +#include +#include #include #include #include @@ -23,12 +25,24 @@ #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" +namespace +{ // utility + +// called from RT control loop +void reset_controller_reference_msg(forward_command_controller::CmdType & msg) +{ + for (auto & data : msg.data) + { + data = std::numeric_limits::quiet_NaN(); + } +} + +} // namespace + namespace forward_command_controller { ForwardControllersBase::ForwardControllersBase() -: controller_interface::ControllerInterface(), - rt_command_ptr_(nullptr), - joints_command_subscriber_(nullptr) +: controller_interface::ControllerInterface(), joints_command_subscriber_(nullptr) { } @@ -58,7 +72,21 @@ controller_interface::CallbackReturn ForwardControllersBase::on_configure( joints_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + [this](const CmdType::SharedPtr msg) + { + const auto cmd = *msg; + + if (!std::all_of( + cmd.data.cbegin(), cmd.data.cend(), + [](const auto & value) { return std::isfinite(value); })) + { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *(get_node()->get_clock()), 1000, + "Non-finite value received. Dropping message"); + return; + } + rt_command_.set(*msg); + }); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -101,7 +129,10 @@ controller_interface::CallbackReturn ForwardControllersBase::on_activate( } // reset command buffer if a command came through callback when controller was inactive - rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + // Try to set default value in command. + // If this fails, then another command will be received soon anyways. + reset_controller_reference_msg(joint_commands_); + rt_command_.try_set(joint_commands_); RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -110,37 +141,50 @@ controller_interface::CallbackReturn ForwardControllersBase::on_activate( controller_interface::CallbackReturn ForwardControllersBase::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - // reset command buffer - rt_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + // Try to set default value in command. + reset_controller_reference_msg(joint_commands_); + rt_command_.try_set(joint_commands_); + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::return_type ForwardControllersBase::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - auto joint_commands = rt_command_ptr_.readFromRT(); + auto joint_commands_op = rt_command_.try_get(); + if (joint_commands_op.has_value()) + { + joint_commands_ = joint_commands_op.value(); + } // no command received yet - if (!joint_commands || !(*joint_commands)) + if (std::all_of( + joint_commands_.data.cbegin(), joint_commands_.data.cend(), + [](const auto & value) { return std::isnan(value); })) { return controller_interface::return_type::OK; } - if ((*joint_commands)->data.size() != command_interfaces_.size()) + if (joint_commands_.data.size() != command_interfaces_.size()) { RCLCPP_ERROR_THROTTLE( get_node()->get_logger(), *(get_node()->get_clock()), 1000, - "command size (%zu) does not match number of interfaces (%zu)", - (*joint_commands)->data.size(), command_interfaces_.size()); + "command size (%zu) does not match number of interfaces (%zu)", joint_commands_.data.size(), + command_interfaces_.size()); return controller_interface::return_type::ERROR; } for (auto index = 0ul; index < command_interfaces_.size(); ++index) { - command_interfaces_[index].set_value((*joint_commands)->data[index]); + if (!command_interfaces_[index].set_value(joint_commands_.data[index])) + { + RCLCPP_WARN( + get_node()->get_logger(), "Unable to set the command interface value %s: value = %f", + command_interfaces_[index].get_name().c_str(), joint_commands_.data[index]); + return controller_interface::return_type::OK; + } } return controller_interface::return_type::OK; } - } // namespace forward_command_controller diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index bf20aa8eb6..50f2f7cf20 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -212,14 +213,14 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command_msg; + command_msg.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command_msg); // update successful, command received ASSERT_EQ( @@ -227,9 +228,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); } TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) @@ -245,9 +246,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command_msg; + command_msg.data = {10.0, 20.0}; + controller_->rt_command_.set(command_msg); // update failed, command size does not match number of joints ASSERT_EQ( @@ -255,9 +256,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); } TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) @@ -277,9 +278,9 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); } TEST_F(ForwardCommandControllerTest, CommandCallbackTest) @@ -290,9 +291,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -323,9 +324,55 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); +} + +TEST_F(ForwardCommandControllerTest, DropInfiniteCommandCallbackTest) +{ + SetUpController(); + + controller_->get_node()->set_parameter({"joints", joint_names_}); + controller_->get_node()->set_parameter({"interface_name", "position"}); + + // default values + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); + + auto node_state = controller_->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + node_state = controller_->get_node()->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // send a new command + rclcpp::Node test_node("test_node"); + auto command_pub = test_node.create_publisher( + std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std_msgs::msg::Float64MultiArray command_msg; + command_msg.data = {10.0, std::numeric_limits::infinity(), 30.0}; + command_pub->publish(command_msg); + + // wait for command message to be passed + 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( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check message containing infinite command value was rejected + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); } TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -336,9 +383,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -362,10 +409,10 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) 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}; - - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + // send command + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful ASSERT_EQ( @@ -373,9 +420,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30); node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -389,35 +436,31 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) 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() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + auto cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // Controller is inactive but let's put some data into buffer (simulate callback when inactive) - command_msg = std::make_shared(); - command_msg->data = {5.5, 6.6, 7.7}; - - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + command.data = {5.5, 6.6, 7.7}; + controller_->rt_command_.set(command); // command ptr should be available and message should be there - same check as in `update` - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each( + ::testing::Not(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN())))); // Now activate again node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - // command ptr should be reset (nullptr) after activation - same check as in `update` - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + // command ptr should be reset after activation - same check as in `update` + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // update successful ASSERT_EQ( @@ -425,12 +468,12 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30); // set commands again - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + controller_->rt_command_.set(command); // update successful ASSERT_EQ( @@ -438,7 +481,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 5.5); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 6.6); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 7.7); } 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 7bd869677c..814d7b6218 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 @@ -15,6 +15,7 @@ /// \authors: Jack Center, Denis Stogl #include +#include #include #include #include @@ -189,9 +190,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateSuccess) SetUpController(true); // check joint commands are the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) @@ -199,9 +200,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) SetUpController(true); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -209,9 +210,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) @@ -224,9 +225,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) @@ -234,9 +235,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) SetUpController(true); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ( @@ -244,9 +245,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) @@ -276,9 +277,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -294,9 +295,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes 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}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -304,9 +305,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); auto node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -319,35 +320,32 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes 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() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + // command ptr should be reset after deactivation - same check as in `update` + auto cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // Controller is inactive but let's put some data into buffer (simulate callback when inactive) - auto command_msg = std::make_shared(); - command_msg->data = {5.5, 6.6, 7.7}; - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + command.data = {5.5, 6.6, 7.7}; + controller_->rt_command_.set(command); // command ptr should be available and message should be there - same check as in `update` - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_TRUE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each( + ::testing::Not(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN())))); // Now activate again node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - // command ptr should be reset (nullptr) after activation - same check as in `update` - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromNonRT() && - *(controller_->rt_command_ptr_.readFromNonRT())); - ASSERT_FALSE( - controller_->rt_command_ptr_.readFromRT() && *(controller_->rt_command_ptr_.readFromRT())); + // command ptr should be reset after activation - same check as in `update` + cmd = controller_->rt_command_.get(); + ASSERT_THAT( + cmd.data, + ::testing::Each(::testing::NanSensitiveDoubleEq(std::numeric_limits::quiet_NaN()))); // update successful ASSERT_EQ( @@ -355,12 +353,12 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0); // set commands again - controller_->rt_command_ptr_.writeFromNonRT(command_msg); + controller_->rt_command_.set(command); // update successful ASSERT_EQ( @@ -368,7 +366,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 5.5); + ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 6.6); + ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 7.7); } diff --git a/gpio_controllers/CHANGELOG.rst b/gpio_controllers/CHANGELOG.rst index f73c54d23d..8eb09683d2 100644 --- a/gpio_controllers/CHANGELOG.rst +++ b/gpio_controllers/CHANGELOG.rst @@ -2,6 +2,52 @@ Changelog for package gpio_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- +* Fix cmake deprecation (backport `#1780 `_) (`#1782 `_) +* Contributors: mergify[bot] + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Add missing github_url to rst files (backport `#1717 `_) (`#1719 `_) +* Fix JSB+GPIO CMakeLists and dependencies (backport `#1705 `_) (`#1707 `_) +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Use smart pointer of ctrl in GpsSensor and GpioCommandController tests (`#1658 `_) diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index 0466dbae19..a19f7586af 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.16) project(gpio_controllers) find_package(ros2_control_cmake REQUIRED) @@ -25,17 +25,17 @@ add_library(gpio_controllers 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 -) +target_link_libraries(gpio_controllers PUBLIC + gpio_command_controller_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${control_msgs_TARGETS} + ${builtin_interfaces_TARGETS}) + pluginlib_export_plugin_description_file(controller_interface gpio_controllers_plugin.xml) install( @@ -63,10 +63,10 @@ if(BUILD_TESTING) ) target_include_directories(test_load_gpio_command_controller PRIVATE include) - ament_target_dependencies(test_load_gpio_command_controller - controller_manager - hardware_interface - ros2_control_test_assets + target_link_libraries(test_load_gpio_command_controller + controller_manager::controller_manager + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets ) ament_add_gmock( @@ -76,15 +76,7 @@ if(BUILD_TESTING) 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 + ros2_control_test_assets::ros2_control_test_assets ) endif() diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index 199a4bd208..a59da906bb 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/gpio_controllers/doc/userdoc.rst + .. _gpio_controllers_userdoc: gpio_controllers diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 35cf7c283d..561abcce91 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -24,8 +24,8 @@ #include "controller_interface/controller_interface.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 "realtime_tools/realtime_thread_safe_box.hpp" #include "gpio_controllers/gpio_command_controller_parameters.hpp" @@ -91,11 +91,16 @@ class GpioCommandController : public controller_interface::ControllerInterface MapOfReferencesToCommandInterfaces command_interfaces_map_; MapOfReferencesToStateInterfaces state_interfaces_map_; - realtime_tools::RealtimeBuffer> rt_command_ptr_{}; rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox rt_command_; + // save the last reference in case of unable to get value from box + CmdType gpio_commands_; + std::shared_ptr> gpio_state_publisher_{}; std::shared_ptr> realtime_gpio_state_publisher_{}; + StateType gpio_state_msg_; std::shared_ptr param_listener_{}; gpio_command_controller_parameters::Params params_; diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index cd761658a2..7b3e36db1b 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 4.24.0 + 4.33.1 Controllers to interact with gpios. Bence Magyar diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index fdecf71c30..8ee414d7a0 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -23,7 +23,7 @@ #include "rclcpp/subscription.hpp" namespace -{ +{ // utility template void print_interface(const rclcpp::Logger & logger, const T & command_interfaces) { @@ -33,6 +33,15 @@ void print_interface(const rclcpp::Logger & logger, const T & command_interfaces } } +// called from RT control loop +void reset_controller_reference_msg( + gpio_controllers::CmdType & msg, const std::shared_ptr & node) +{ + msg.header.stamp = node->now(); + msg.interface_groups.clear(); + msg.interface_values.clear(); +} + std::vector extract_gpios_from_hardware_info( const std::vector & hardware_infos) { @@ -85,7 +94,7 @@ try { gpios_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + [this](const CmdType::SharedPtr msg) { rt_command_.set(*msg); }); } gpio_state_publisher_ = @@ -137,14 +146,18 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State } initialize_gpio_state_msg(); - rt_command_ptr_.reset(); + // Set default value in command + reset_controller_reference_msg(gpio_commands_, get_node()); + rt_command_.try_set(gpio_commands_); RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return CallbackReturn::SUCCESS; } CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) { - rt_command_ptr_.reset(); + // Set default value in command + reset_controller_reference_msg(gpio_commands_, get_node()); + rt_command_.try_set(gpio_commands_); return CallbackReturn::SUCCESS; } @@ -239,19 +252,18 @@ void GpioCommandController::store_state_interface_types() 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()); + 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 = + 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(), + 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()); } } @@ -301,7 +313,7 @@ bool GpioCommandController::check_if_configured_interfaces_matches_received( if (!(configured_interfaces.size() == interfaces_from_params.size())) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %ld interfaces, got %ld", interfaces_from_params.size(), + get_node()->get_logger(), "Expected %zu interfaces, got %zu", interfaces_from_params.size(), configured_interfaces.size()); for (const auto & interface : interfaces_from_params) { @@ -315,19 +327,24 @@ bool GpioCommandController::check_if_configured_interfaces_matches_received( controller_interface::return_type GpioCommandController::update_gpios_commands() { - auto gpio_commands_ptr = rt_command_ptr_.readFromRT(); - if (!gpio_commands_ptr || !(*gpio_commands_ptr)) + auto gpio_commands_op = rt_command_.try_get(); + if (gpio_commands_op.has_value()) { + gpio_commands_ = gpio_commands_op.value(); + } + if (gpio_commands_.interface_groups.empty() || gpio_commands_.interface_values.empty()) + { + // no command received yet 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) + 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]; + 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()) + 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", @@ -335,10 +352,10 @@ controller_interface::return_type GpioCommandController::update_gpios_commands() 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 < gpio_commands_.interface_values[gpio_index].values.size(); ++command_interface_index) { - apply_command(gpio_commands, gpio_index, command_interface_index); + apply_command(gpio_commands_, gpio_index, command_interface_index); } } return controller_interface::return_type::OK; @@ -350,11 +367,18 @@ void GpioCommandController::apply_command( const auto full_command_interface_name = gpio_commands.interface_groups[gpio_index] + '/' + gpio_commands.interface_values[gpio_index].interface_names[command_interface_index]; + + const auto & command_value = + gpio_commands.interface_values[gpio_index].values[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]); + if (!command_interfaces_map_.at(full_command_interface_name).get().set_value(command_value)) + { + RCLCPP_WARN( + get_node()->get_logger(), "Unable to set the command for interface '%s' with value '%f'.", + full_command_interface_name.c_str(), command_value); + } } catch (const std::exception & e) { @@ -366,24 +390,23 @@ void GpioCommandController::apply_command( void GpioCommandController::update_gpios_states() { - if (!realtime_gpio_state_publisher_ || !realtime_gpio_state_publisher_->trylock()) + if (!realtime_gpio_state_publisher_) { 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_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 < gpio_state_msg_.interface_values[gpio_index].interface_names.size(); ++interface_index) { - apply_state_value(gpio_state_msg, gpio_index, interface_index); + apply_state_value(gpio_state_msg_, gpio_index, interface_index); } } - realtime_gpio_state_publisher_->unlockAndPublish(); + realtime_gpio_state_publisher_->try_publish(gpio_state_msg_); } void GpioCommandController::apply_state_value( @@ -394,8 +417,20 @@ void GpioCommandController::apply_state_value( 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(); + auto state_msg_interface_value_op = + state_interfaces_map_.at(interface_name).get().get_optional(); + + if (!state_msg_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Unable to retrieve the data from state: %s \n", + interface_name.c_str()); + } + else + { + state_msg.interface_values[gpio_index].values[interface_index] = + state_msg_interface_value_op.value(); + } } catch (const std::exception & e) { diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index de085894f2..9bf26e91d9 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -133,12 +133,12 @@ class GpioCommandControllerTestSuite : public ::testing::Test 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)); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands.at(2)); + ASSERT_EQ(gpio_1_1_dig_state.get_optional().value(), gpio_states.at(0)); + ASSERT_EQ(gpio_1_2_dig_state.get_optional().value(), gpio_states.at(1)); + ASSERT_EQ(gpio_2_ana_state.get_optional().value(), gpio_states.at(2)); } void update_controller_loop() @@ -445,7 +445,7 @@ TEST_F( 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)); + controller_->rt_command_.set(command); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); @@ -467,7 +467,7 @@ TEST_F( 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)); + controller_->rt_command_.set(command); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); @@ -489,12 +489,12 @@ TEST_F( 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)); + controller_->rt_command_.set(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); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0); } TEST_F( @@ -513,12 +513,12 @@ TEST_F( 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)); + controller_->rt_command_.set(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); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0); } TEST_F( @@ -536,13 +536,12 @@ TEST_F( const auto command = createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); - - controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_.set(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]); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands[2]); } TEST_F( @@ -561,13 +560,12 @@ TEST_F( 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)); + controller_->rt_command_.set(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)); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands.at(2)); } TEST_F( @@ -592,9 +590,9 @@ TEST_F( 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); + ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0); } TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurrentValues) diff --git a/gps_sensor_broadcaster/CHANGELOG.rst b/gps_sensor_broadcaster/CHANGELOG.rst index 95847008d5..4e90b9d1ff 100644 --- a/gps_sensor_broadcaster/CHANGELOG.rst +++ b/gps_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,48 @@ Changelog for package gps_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- +* Fix temporary copies of other semantic components (backport `#1905 `_) (`#1908 `_) +* Contributors: mergify[bot] + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Use smart pointer of ctrl in GpsSensor and GpioCommandController tests (`#1658 `_) diff --git a/gps_sensor_broadcaster/CMakeLists.txt b/gps_sensor_broadcaster/CMakeLists.txt index 80928a223d..7cc818d1fe 100644 --- a/gps_sensor_broadcaster/CMakeLists.txt +++ b/gps_sensor_broadcaster/CMakeLists.txt @@ -40,8 +40,15 @@ target_include_directories(gps_sensor_broadcaster PUBLIC $ ) -target_link_libraries(gps_sensor_broadcaster PUBLIC gps_sensor_broadcaster_parameters) -ament_target_dependencies(gps_sensor_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(gps_sensor_broadcaster PUBLIC + gps_sensor_broadcaster_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${sensor_msgs_TARGETS}) pluginlib_export_plugin_description_file( controller_interface gps_sensor_broadcaster.xml) @@ -59,11 +66,8 @@ if(BUILD_TESTING) target_include_directories(test_load_gps_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_gps_sensor_broadcaster gps_sensor_broadcaster - ) - ament_target_dependencies(test_load_gps_sensor_broadcaster - controller_manager - hardware_interface - ros2_control_test_assets + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) ament_add_gmock(test_gps_sensor_broadcaster @@ -72,10 +76,7 @@ if(BUILD_TESTING) target_include_directories(test_gps_sensor_broadcaster PRIVATE include) target_link_libraries(test_gps_sensor_broadcaster gps_sensor_broadcaster - ) - ament_target_dependencies(test_gps_sensor_broadcaster - hardware_interface - ros2_control_test_assets + ros2_control_test_assets::ros2_control_test_assets ) endif() diff --git a/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp b/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp index 35c88073aa..2ef100af25 100644 --- a/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp +++ b/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp @@ -57,8 +57,11 @@ class GPSSensorBroadcaster : public controller_interface::ControllerInterface callback_return_type setup_publisher(); GPSSensorVariant gps_sensor_; + rclcpp::Publisher::SharedPtr sensor_state_publisher_; std::unique_ptr realtime_publisher_; + sensor_msgs::msg::NavSatFix state_message_; + std::shared_ptr param_listener_{}; gps_sensor_broadcaster::Params params_; std::vector state_names_; diff --git a/gps_sensor_broadcaster/package.xml b/gps_sensor_broadcaster/package.xml index e65d25429f..9557247210 100644 --- a/gps_sensor_broadcaster/package.xml +++ b/gps_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ gps_sensor_broadcaster - 4.24.0 + 4.33.1 Controller to publish readings of GPS sensors. Apache License 2.0 diff --git a/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp index 182eb0e87d..0856987f72 100644 --- a/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp +++ b/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp @@ -56,12 +56,16 @@ callback_return_type GPSSensorBroadcaster::on_configure(const rclcpp_lifecycle:: param_listener_->refresh_dynamic_parameters(); params_ = param_listener_->get_params(); - gps_sensor_ = - params_.read_covariance_from_interface - ? GPSSensorVariant{semantic_components::GPSSensor( - params_.sensor_name)} - : GPSSensorVariant{ - semantic_components::GPSSensor(params_.sensor_name)}; + if (params_.read_covariance_from_interface) + { + gps_sensor_.emplace>( + params_.sensor_name); + } + else + { + gps_sensor_.emplace>( + params_.sensor_name); + } std::visit( Visitor{ [this](auto & sensor) { state_names_ = sensor.get_state_interface_names(); }, @@ -78,10 +82,8 @@ callback_return_type GPSSensorBroadcaster::setup_publisher() sensor_state_publisher_ = get_node()->create_publisher( "~/gps/fix", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(sensor_state_publisher_); - realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = params_.frame_id; + state_message_.header.frame_id = params_.frame_id; setup_covariance(); - realtime_publisher_->unlock(); return callback_return_type::SUCCESS; } @@ -98,14 +100,14 @@ void GPSSensorBroadcaster::setup_covariance() { if (params_.read_covariance_from_interface) { - realtime_publisher_->msg_.position_covariance_type = COVARIANCE_TYPE_DIAGONAL_KNOWN; + state_message_.position_covariance_type = COVARIANCE_TYPE_DIAGONAL_KNOWN; return; } for (size_t i = 0; i < COVARIANCE_3x3_SIZE; ++i) { - realtime_publisher_->msg_.position_covariance[i] = params_.static_position_covariance[i]; - realtime_publisher_->msg_.position_covariance_type = COVARIANCE_TYPE_KNOWN; + state_message_.position_covariance[i] = params_.static_position_covariance[i]; + state_message_.position_covariance_type = COVARIANCE_TYPE_KNOWN; } } @@ -148,19 +150,19 @@ callback_return_type GPSSensorBroadcaster::on_deactivate(const rclcpp_lifecycle: controller_interface::return_type GPSSensorBroadcaster::update( const rclcpp::Time &, const rclcpp::Duration &) { - if (realtime_publisher_ && realtime_publisher_->trylock()) + if (realtime_publisher_) { - realtime_publisher_->msg_.header.stamp = get_node()->now(); + state_message_.header.stamp = get_node()->now(); std::visit( Visitor{ [this](auto & sensor) { sensor_msgs::msg::NavSatFix message; - sensor.get_values_as_message(realtime_publisher_->msg_); + sensor.get_values_as_message(state_message_); }, [](std::monostate &) {}}, gps_sensor_); - realtime_publisher_->unlockAndPublish(); + realtime_publisher_->try_publish(state_message_); } return controller_interface::return_type::OK; } diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 8c0b5cd98e..844b615bb6 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,46 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- +* Add deprecation notice of gripper_action_controller to docs (`#1852 `_) +* Contributors: Christoph Fröhlich + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- diff --git a/gripper_controllers/doc/userdoc.rst b/gripper_controllers/doc/userdoc.rst index 7f51c8f4ac..ffaeefca11 100644 --- a/gripper_controllers/doc/userdoc.rst +++ b/gripper_controllers/doc/userdoc.rst @@ -10,6 +10,10 @@ Controllers for executing a gripper command action for simple single-dof gripper - ``position_controllers/GripperActionController`` - ``effort_controllers/GripperActionController`` +.. note:: + + The ``effort_controllers/GripperActionController`` and ``position_controllers/GripperActionController`` will be removed. The ``parallel_gripper_action_controller/GripperActionController`` should be used instead. `(#1652 `__). + 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. 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 a94e163d7b..8e60a1add0 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -65,8 +65,8 @@ controller_interface::return_type GripperActionController::up { command_struct_rt_ = *(command_.readFromRT()); - const double current_position = joint_position_state_interface_->get().get_value(); - const double current_velocity = joint_velocity_state_interface_->get().get_value(); + const double current_position = joint_position_state_interface_->get().get_optional().value(); + const double current_velocity = joint_velocity_state_interface_->get().get_optional().value(); const double error_position = command_struct_rt_.position_ - current_position; const double error_velocity = -current_velocity; @@ -147,7 +147,7 @@ rclcpp_action::CancelResponse GripperActionController::cancel template void GripperActionController::set_hold_position() { - command_struct_.position_ = joint_position_state_interface_->get().get_value(); + command_struct_.position_ = joint_position_state_interface_->get().get_optional().value(); command_struct_.max_effort_ = params_.max_effort; command_.writeFromNonRT(command_struct_); } @@ -285,7 +285,7 @@ controller_interface::CallbackReturn GripperActionController: hw_iface_adapter_.init(joint_command_interface_, get_node()); // Command - non RT version - command_struct_.position_ = joint_position_state_interface_->get().get_value(); + command_struct_.position_ = joint_position_state_interface_->get().get_optional().value(); command_struct_.max_effort_ = params_.max_effort; command_.initRT(command_struct_); diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 66da3d869a..1c7d08baaf 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -83,7 +83,7 @@ class HardwareInterfaceAdapter double /* error_velocity */, double max_allowed_effort) { // Forward desired position to command - joint_handle_->get().set_value(desired_position); + std::ignore = joint_handle_->get().set_value(desired_position); return max_allowed_effort; } @@ -148,7 +148,7 @@ class HardwareInterfaceAdapter } // Reset PIDs, zero effort commands pid_->reset(); - joint_handle_->get().set_value(0.0); + std::ignore = joint_handle_->get().set_value(0.0); } void stopping(const rclcpp::Time & /* time */) {} @@ -168,7 +168,7 @@ class HardwareInterfaceAdapter double command = pid_->compute_command(error_position, error_velocity, period); command = std::min( fabs(max_allowed_effort), std::max(-fabs(max_allowed_effort), command)); - joint_handle_->get().set_value(command); + std::ignore = joint_handle_->get().set_value(command); last_update_time_ = std::chrono::steady_clock::now(); return command; } diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index d7decaac37..c8f726044c 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.24.0 + 4.33.1 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 1d469d5d89..290ced66c4 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,48 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- +* Fix temporary copies of other semantic components (backport `#1905 `_) (`#1908 `_) +* Contributors: mergify[bot] + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index e63f0bb328..9d7727e33d 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -35,9 +35,14 @@ target_include_directories(imu_sensor_broadcaster PUBLIC $ ) target_link_libraries(imu_sensor_broadcaster PUBLIC - imu_sensor_broadcaster_parameters -) -ament_target_dependencies(imu_sensor_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + imu_sensor_broadcaster_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${sensor_msgs_TARGETS}) pluginlib_export_plugin_description_file( controller_interface imu_sensor_broadcaster.xml) @@ -53,11 +58,8 @@ if(BUILD_TESTING) 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 + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) add_rostest_with_parameters_gmock(test_imu_sensor_broadcaster @@ -67,9 +69,6 @@ if(BUILD_TESTING) target_link_libraries(test_imu_sensor_broadcaster imu_sensor_broadcaster ) - ament_target_dependencies(test_imu_sensor_broadcaster - hardware_interface - ) endif() install( diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 14911df8b0..103c001c27 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.24.0 + 4.33.1 Controller to publish readings of IMU sensors. Bence Magyar diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 04a28e5368..9d55e5cde3 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -45,8 +45,7 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( { params_ = param_listener_->get_params(); - imu_sensor_ = std::make_unique( - semantic_components::IMUSensor(params_.sensor_name)); + imu_sensor_ = std::make_unique(params_.sensor_name); try { // register ft sensor data publisher diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 67cf31f8a5..9cd3219495 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -35,7 +35,6 @@ using testing::SizeIs; namespace { constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; -constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace void IMUSensorBroadcasterTest::SetUpTestCase() {} diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 7c8de6e944..eada361763 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,51 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- +* Added frame_id to Joint State Broadcaster (backport `#1746 `_) (`#1897 `_) +* Contributors: mergify[bot] + +4.31.0 (2025-08-27) +------------------- +* docs(joint_state_broadcaster): clarify /dynamic_joint_states contents (backport `#1865 `_) (`#1871 `_) +* Contributors: mergify[bot] + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Fix JSB+GPIO CMakeLists and dependencies (backport `#1705 `_) (`#1707 `_) +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Call `configure()` of base class instead of node (`#1659 `_) diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 54a033e553..d7f5890659 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -12,7 +12,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS generate_parameter_library pluginlib rclcpp_lifecycle - rcutils realtime_tools sensor_msgs urdf @@ -37,9 +36,16 @@ 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}) + joint_state_broadcaster_parameters + controller_interface::controller_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + urdf::urdf + ${sensor_msgs_TARGETS} + ${control_msgs_TARGETS} + ${builtin_interfaces_TARGETS}) pluginlib_export_plugin_description_file(controller_interface joint_state_plugin.xml) if(BUILD_TESTING) @@ -54,23 +60,17 @@ if(BUILD_TESTING) ) 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 - ) + controller_manager::controller_manager + hardware_interface::hardware_interface + rclcpp::rclcpp + ros2_control_test_assets::ros2_control_test_assets) ament_add_gmock(test_joint_state_broadcaster test/test_joint_state_broadcaster.cpp ) target_link_libraries(test_joint_state_broadcaster joint_state_broadcaster - ) - ament_target_dependencies(test_joint_state_broadcaster - hardware_interface - ros2_control_test_assets - ) + ros2_control_test_assets::ros2_control_test_assets) endif() install( diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index 996284d51a..151b711dc4 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -20,6 +20,42 @@ In the latter case, resulting interfaces is defined by a "matrix" of interfaces If some requested interfaces are missing, the controller will print a warning about that, but work for other interfaces. If none of the requested interface are not defined, the controller returns error on activation. +Published topics +---------------- + +* ``/joint_states`` (``sensor_msgs/msg/JointState``): + + Publishes *movement-related* interfaces only — ``position``, ``velocity``, + and ``effort`` — for joints that provide them. If a joint does not expose a given + movement interface, that field is omitted/left empty for that joint in the message. + +* ``/dynamic_joint_states`` (``control_msgs/msg/DynamicJointState``): + + Publishes **all available state interfaces** for each joint. This includes the + movement interfaces (position/velocity/effort) *and* any additional or custom + interfaces provided by the hardware (e.g., temperature, voltage, torque sensor + readings, calibration flags). + + The message maps ``joint_names`` to per-joint interface name lists and values. + Example payload:: + + joint_names: [joint1, joint2] + interface_values: + - interface_names: [position, velocity, effort] + values: [1.5708, 0.0, 0.20] + - interface_names: [position, temperature] + values: [0.7854, 42.1] + + Use this topic if you need *every* reported interface, not just movement. + +.. note:: + + If ``use_local_topics`` is set to ``true``, both topics are published in the + controller’s namespace (e.g., ``/my_state_broadcaster/joint_states`` and + ``/my_state_broadcaster/dynamic_joint_states``). If ``false`` (default), + they are published at the root (e.g., ``/joint_states``). + + 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. 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 f12c192831..05d2334d0b 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 @@ -97,12 +97,15 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface Params params_; std::unordered_map map_interface_to_joint_state_; + std::string frame_id_; + // For the JointState message, // we store the name of joints with compatible interfaces std::vector joint_names_; std::shared_ptr> joint_state_publisher_; std::shared_ptr> realtime_joint_state_publisher_; + sensor_msgs::msg::JointState joint_state_msg_; // For the DynamicJointState format, we use a map to buffer values in for easier lookup // This allows to preserve whatever order or names/interfaces were initialized. @@ -111,6 +114,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface dynamic_joint_state_publisher_; std::shared_ptr> realtime_dynamic_joint_state_publisher_; + control_msgs::msg::DynamicJointState dynamic_joint_state_msg_; urdf::Model model_; bool is_model_loaded_ = false; diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index a3bea0f44b..a0136fe284 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.24.0 + 4.33.1 Broadcaster to publish joint state Bence Magyar @@ -30,7 +30,6 @@ pluginlib rclcpp_lifecycle rclcpp - rcutils realtime_tools sensor_msgs urdf diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index c6857cf245..bdcc1d1c82 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -175,11 +175,16 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( (params_.joints.empty() ? model_.joints_.size() : params_.joints.size()) + params_.extra_joints.size(); joint_names_.reserve(max_joints_size); - auto & joint_state_msg = realtime_joint_state_publisher_->msg_; - joint_state_msg.name.reserve(max_joints_size); - joint_state_msg.position.reserve(max_joints_size); - joint_state_msg.velocity.reserve(max_joints_size); - joint_state_msg.effort.reserve(max_joints_size); + joint_state_msg_.name.reserve(max_joints_size); + joint_state_msg_.position.reserve(max_joints_size); + joint_state_msg_.velocity.reserve(max_joints_size); + joint_state_msg_.effort.reserve(max_joints_size); + + frame_id_ = params_.frame_id; + if (frame_id_.empty()) + { + RCLCPP_WARN(get_node()->get_logger(), "Frame ID is not set."); + } return CallbackReturn::SUCCESS; } @@ -308,15 +313,15 @@ void JointStateBroadcaster::init_joint_state_msg() { const size_t num_joints = joint_names_.size(); - /// @note joint_state_msg publishes position, velocity and effort for all joints, + /// @note joint_state_msg_ publishes position, velocity and effort for all joints, /// with at least one of these interfaces, the rest are omitted from this message // default initialization for joint state message - auto & joint_state_msg = realtime_joint_state_publisher_->msg_; - joint_state_msg.name = joint_names_; - joint_state_msg.position.resize(num_joints, kUninitializedValue); - joint_state_msg.velocity.resize(num_joints, kUninitializedValue); - joint_state_msg.effort.resize(num_joints, kUninitializedValue); + joint_state_msg_.header.frame_id = frame_id_; + joint_state_msg_.name = joint_names_; + joint_state_msg_.position.resize(num_joints, kUninitializedValue); + joint_state_msg_.velocity.resize(num_joints, kUninitializedValue); + joint_state_msg_.effort.resize(num_joints, kUninitializedValue); // save joint state data auto get_address = @@ -345,35 +350,36 @@ 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(); + dynamic_joint_state_msg_.header.frame_id = frame_id_; + 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; const auto & interfaces_and_values = name_ifv.second; - dynamic_joint_state_msg.joint_names.push_back(name); + dynamic_joint_state_msg_.joint_names.push_back(name); control_msgs::msg::InterfaceValue if_value; for (const auto & interface_and_value : interfaces_and_values) { if_value.interface_names.emplace_back(interface_and_value.first); if_value.values.emplace_back(kUninitializedValue); } - dynamic_joint_state_msg.interface_values.emplace_back(if_value); + dynamic_joint_state_msg_.interface_values.emplace_back(if_value); } // save dynamic joint state data dynamic_joint_states_data_.clear(); - const auto & msg = realtime_dynamic_joint_state_publisher_->msg_; - for (auto ji = 0u; ji < msg.joint_names.size(); ++ji) + for (auto ji = 0u; ji < dynamic_joint_state_msg_.joint_names.size(); ++ji) { dynamic_joint_states_data_.push_back(std::vector()); - const auto & name = msg.joint_names[ji]; + const auto & name = dynamic_joint_state_msg_.joint_names[ji]; - for (auto ii = 0u; ii < msg.interface_values[ji].interface_names.size(); ++ii) + for (auto ii = 0u; ii < dynamic_joint_state_msg_.interface_values[ji].interface_names.size(); + ++ii) { - const auto & interface_name = msg.interface_values[ji].interface_names[ii]; + const auto & interface_name = + dynamic_joint_state_msg_.interface_values[ji].interface_names[ii]; dynamic_joint_states_data_[ji].push_back(&name_if_value_mapping_[name][interface_name]); } } @@ -413,34 +419,33 @@ controller_interface::return_type JointStateBroadcaster::update( } } - if (realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock()) + if (realtime_joint_state_publisher_) { - auto & joint_state_msg = realtime_joint_state_publisher_->msg_; - - joint_state_msg.header.stamp = time; + joint_state_msg_.header.stamp = time; // update joint state message and dynamic joint state message for (size_t i = 0; i < joint_names_.size(); ++i) { - joint_state_msg.position[i] = joint_states_data_[i].position_; - joint_state_msg.velocity[i] = joint_states_data_[i].velocity_; - joint_state_msg.effort[i] = joint_states_data_[i].effort_; + joint_state_msg_.position[i] = joint_states_data_[i].position_; + joint_state_msg_.velocity[i] = joint_states_data_[i].velocity_; + joint_state_msg_.effort[i] = joint_states_data_[i].effort_; } - realtime_joint_state_publisher_->unlockAndPublish(); + realtime_joint_state_publisher_->try_publish(joint_state_msg_); } - if (realtime_dynamic_joint_state_publisher_ && realtime_dynamic_joint_state_publisher_->trylock()) + if (realtime_dynamic_joint_state_publisher_) { - auto & msg = realtime_dynamic_joint_state_publisher_->msg_; - msg.header.stamp = time; - for (auto ji = 0u; ji < msg.joint_names.size(); ++ji) + dynamic_joint_state_msg_.header.stamp = time; + for (auto ji = 0u; ji < dynamic_joint_state_msg_.joint_names.size(); ++ji) { - for (auto ii = 0u; ii < msg.interface_values[ji].interface_names.size(); ++ii) + for (auto ii = 0u; ii < dynamic_joint_state_msg_.interface_values[ji].interface_names.size(); + ++ii) { - msg.interface_values[ji].values[ii] = *dynamic_joint_states_data_[ji][ii]; + dynamic_joint_state_msg_.interface_values[ji].values[ii] = + *dynamic_joint_states_data_[ji][ii]; } } - realtime_dynamic_joint_state_publisher_->unlockAndPublish(); + realtime_dynamic_joint_state_publisher_->try_publish(dynamic_joint_state_msg_); } return controller_interface::return_type::OK; diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index c8ca928a12..ae525b42a8 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -46,3 +46,8 @@ joint_state_broadcaster: If true, the broadcaster will publish the data of the joints present in the URDF alone. If false, the broadcaster will publish the data of any interface that has type ``position``, ``velocity``, or ``effort``." } + frame_id: { + type: string, + default_value: "base_link", + description: "The frame_id to be used in the published joint states. This parameter allows rviz2 to visualize the effort of the joints." + } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 277285b05c..b963b520c3 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -77,6 +77,7 @@ void JointStateBroadcasterTest::init_broadcaster_and_set_parameters( state_broadcaster_->get_node()->set_parameter({"joints", joint_names}); state_broadcaster_->get_node()->set_parameter({"interfaces", interfaces}); + state_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); } void JointStateBroadcasterTest::assign_state_interfaces( @@ -176,15 +177,16 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); 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_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); 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_)); @@ -226,15 +228,16 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); 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_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); 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_)); @@ -271,15 +274,16 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces 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_; + const auto & new_joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(new_joint_state_msg.header.frame_id, frame_id_); 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_; + const auto & new_dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(new_dynamic_joint_state_msg.header.frame_id, frame_id_); 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)); @@ -321,15 +325,16 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); 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_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); 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_)); @@ -371,15 +376,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_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_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_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_)); @@ -426,15 +430,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDes ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_in_urdf)); 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 and it will have the data of all the interfaces - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size())); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size())); ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); @@ -480,15 +483,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_in_urdf)); 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 and it will have the data of all the interfaces - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size())); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size())); ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); @@ -533,15 +535,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndInterfaces) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_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 and it will have the data of all the interfaces - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_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)); @@ -576,15 +577,16 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); 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_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); 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_)); @@ -625,7 +627,8 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); 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)); @@ -640,8 +643,8 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) } // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); 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)); @@ -689,7 +692,8 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); 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)); @@ -700,8 +704,8 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) } // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); 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)); @@ -765,7 +769,8 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); 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)); @@ -778,8 +783,8 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) } // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); 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)); @@ -812,15 +817,16 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) 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_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, SizeIs(0)); ASSERT_THAT(joint_state_msg.position, SizeIs(0)); ASSERT_THAT(joint_state_msg.velocity, SizeIs(0)); ASSERT_THAT(joint_state_msg.effort, SizeIs(0)); // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); 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)); @@ -857,7 +863,8 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) 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_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); 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)); @@ -872,8 +879,8 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) } // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); 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)); @@ -901,6 +908,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate) const size_t NUM_JOINTS = JOINT_NAMES.size(); // joint state initialized + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); 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_); @@ -916,8 +924,8 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate) } // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); 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)); @@ -1004,7 +1012,7 @@ TEST_F(JointStateBroadcasterTest, UpdatePerformanceTest) } RCLCPP_INFO( - state_broadcaster_->get_node()->get_logger(), "Number of test interfaces: %lu", + state_broadcaster_->get_node()->get_logger(), "Number of test interfaces: %zu", test_interfaces_.size()); std::vector state_interfaces; @@ -1107,6 +1115,7 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st activate_and_get_joint_state_message(topic, joint_state_msg); const size_t NUM_JOINTS = joint_names_.size(); + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, 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 @@ -1177,6 +1186,7 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( const size_t NUM_JOINTS = 3; const std::vector INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; + ASSERT_EQ(dynamic_joint_state_msg->header.frame_id, frame_id_); 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 @@ -1234,14 +1244,15 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest) const size_t NUM_JOINTS = all_joint_names.size(); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; + ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, ElementsAreArray(all_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_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; + ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index a167d33bf5..158c921ddc 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -112,6 +112,7 @@ class JointStateBroadcasterTest : public ::testing::Test std::vector test_interfaces_; std::unique_ptr state_broadcaster_; + std::string frame_id_ = "base_link"; }; #endif // TEST_JOINT_STATE_BROADCASTER_HPP_ diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 881fe3b12f..edaa2d700c 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,66 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- +* Fix upstream Pid class deprecation warnings (`#1959 `_) +* Fix JTC crashing when shutdown while executing (backport `#1960 `_) (`#1962 `_) +* Remove unused `get_state_msg` method (backport `#1949 `_) (`#1950 `_) +* Don't use `msg\_` field of realtime publisher (backport `#1947 `_) (`#1948 `_) +* Contributors: Christoph Fröhlich, mergify[bot] + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Remove wrong and unnecessary docstrings (backport `#1912 `_) (`#1924 `_) +* Use auto dependency management for windows workflow (backport `#1917 `_) (`#1922 `_) +* Remove unused variables and correctly override test class method (backport `#1918 `_) (`#1920 `_) +* Don't call `release_interfaces` from controllers (backport `#1910 `_) (`#1911 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- +* Preallocate `std::vector` variables for interfaces (backport `#1893 `_) (`#1899 `_) +* Contributors: mergify[bot] + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- +* Reset JTC PID's to zero on_activate() (backport `#1840 `_) (`#1844 `_) +* Contributors: mergify[bot] + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- +* Fix atomic variables in JTC (backport `#1749 `_) (`#1766 `_) +* Add new AntiWindup parameters to JTC (backport `#1759 `_) (`#1762 `_) +* [JTC Doc] Update PID documentation for effort related command interface support (backport `#1748 `_) (`#1752 `_) +* Contributors: Julia Jia, Christoph Fröhlich, Sai Kishor Kothakota + +4.26.0 (2025-06-06) +------------------- +* JTC: Use std::atomic (backport `#1720 `_) (`#1723 `_) +* Reset both sec and nanosec in time_from_start (backport `#1709 `_) (`#1711 `_) +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Call `configure()` of base class instead of node (`#1659 `_) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 58a6378e19..19ac350e10 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -38,14 +38,28 @@ add_library(joint_trajectory_controller SHARED src/trajectory.cpp ) target_compile_features(joint_trajectory_controller PUBLIC cxx_std_17) +if(WIN32) + target_compile_definitions(joint_trajectory_controller PRIVATE _USE_MATH_DEFINES) +endif() 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}) + joint_trajectory_controller_parameters + control_toolbox::control_toolbox + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + rsl::rsl + tl_expected::tl_expected + urdf::urdf + angles::angles + ${trajectory_msgs_TARGETS} + ${control_msgs_TARGETS}) pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) @@ -55,23 +69,26 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_trajectory test/test_trajectory.cpp) - target_link_libraries(test_trajectory joint_trajectory_controller) - ament_target_dependencies(test_trajectory ros2_control_test_assets) + target_link_libraries(test_trajectory + joint_trajectory_controller + ros2_control_test_assets::ros2_control_test_assets + ) 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) - ament_target_dependencies(test_tolerances ros2_control_test_assets) - target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES) + target_link_libraries(test_tolerances + joint_trajectory_controller + ros2_control_test_assets::ros2_control_test_assets + ) ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) target_link_libraries(test_trajectory_controller joint_trajectory_controller + ros2_control_test_assets::ros2_control_test_assets ) - ament_target_dependencies(test_trajectory_controller ros2_control_test_assets) - target_compile_definitions(joint_trajectory_controller PRIVATE _USE_MATH_DEFINES) + target_compile_definitions(test_trajectory_controller PRIVATE _USE_MATH_DEFINES) add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") ament_add_gmock(test_load_joint_trajectory_controller @@ -79,12 +96,8 @@ if(BUILD_TESTING) ) target_link_libraries(test_load_joint_trajectory_controller joint_trajectory_controller - ) - ament_target_dependencies(test_load_joint_trajectory_controller - controller_manager - control_toolbox - realtime_tools - ros2_control_test_assets + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) ament_add_gmock(test_trajectory_actions @@ -93,8 +106,7 @@ if(BUILD_TESTING) set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 300) target_link_libraries(test_trajectory_actions joint_trajectory_controller - ) - ament_target_dependencies(test_trajectory_actions ros2_control_test_assets) + ros2_control_test_assets::ros2_control_test_assets) endif() diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml index 9623899245..9d3f8753f2 100644 --- a/joint_trajectory_controller/doc/parameters_context.yaml +++ b/joint_trajectory_controller/doc/parameters_context.yaml @@ -2,17 +2,29 @@ constraints: Default values for tolerances if no explicit values are set in the ``JointTrajectory`` message. gains: | - 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 + The parameters are used to configure PID loops for the ``velocity`` or ``effort``-only command interfaces. This structure contains the controller gains for every joint with following the control laws - .. math:: u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) + * If ``velocity`` is the only command interface: - with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), - the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. + .. 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 below), + the controller period :math:`dt`, and the ``velocity`` manipulated variable (control variable) :math:`u`, respectively. + + * If ``effort`` is the only command interface: + + .. math:: + + u = k_{ff} v_d + \delta_d + k_p e + k_i \sum e dt + k_d (v_d - v) + + with the desired velocity :math:`v_d`, the desired effort :math:`\delta_d` if provided in the trajectory (or 0 otherwise), the measured velocity :math:`v`, the position error :math:`e` (definition see below), + the controller period :math:`dt`, and the ``effort`` manipulated variable (control variable) :math:`u`, respectively. If the joint is of continuous type, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`, i.e., the shortest rotation to the target position is the desired motion. 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. - If you want to turn off the PIDs (open loop control), set the feedback gains to zero. + If you want to turn off the PIDs (open loop control), set the feedback gains to zero and an appropriate value for feed-forward gain :math:`k_{ff}`. diff --git a/joint_trajectory_controller/doc/trajectory.rst b/joint_trajectory_controller/doc/trajectory.rst index d570d59fab..71e7cf831e 100644 --- a/joint_trajectory_controller/doc/trajectory.rst +++ b/joint_trajectory_controller/doc/trajectory.rst @@ -111,60 +111,7 @@ To visualize the difference of the different interpolation methods and their inp 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 `_ +The current implementation just forgets the old trajectory. +Follow this `issue `__ for more information. diff --git a/joint_trajectory_controller/doc/trajectory_replacement_future.png b/joint_trajectory_controller/doc/trajectory_replacement_future.png deleted file mode 100644 index 582a6acce8..0000000000 Binary files a/joint_trajectory_controller/doc/trajectory_replacement_future.png and /dev/null differ diff --git a/joint_trajectory_controller/doc/trajectory_replacement_now.png b/joint_trajectory_controller/doc/trajectory_replacement_now.png deleted file mode 100644 index a7cd2f6bc7..0000000000 Binary files a/joint_trajectory_controller/doc/trajectory_replacement_now.png and /dev/null differ diff --git a/joint_trajectory_controller/doc/trajectory_replacement_past.png b/joint_trajectory_controller/doc/trajectory_replacement_past.png deleted file mode 100644 index a719f6fc66..0000000000 Binary files a/joint_trajectory_controller/doc/trajectory_replacement_past.png and /dev/null differ diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index c1a244bc88..357afbabf1 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -30,9 +30,9 @@ This means that the joints can have one or more command interfaces, where the fo * 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`). -* For ``effort`` command interface (without ``position`` command interface), if the trajectory contains effort, this will be added to the PID commands as a feed forward effort. -* For ``position, effort`` command interface, if the trajectory contains effort, this will be passed directly to the ``effort`` interface (PID won't be used) while the positions will be passed to the ``position`` interface. +* For ``velocity`` command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` commands through a PID loop if it is configured (:ref:`parameters`). +* For ``effort`` command interface (without ``position`` command interface), the position+velocity trajectory following error is mapped to ``effort`` commands through a PID loop if it is configured (:ref:`parameters`). In addition, it adds trajectory's effort as feedforward effort to the PID output. +* For ``position, effort`` command interface, PID loop is not used. If the trajectory contains effort, its value will be passed directly to the ``effort`` interface while the desired positions will be forwarded to the ``position`` interface. This could be useful for manipulation tasks where you need to add that extra force to maintain contact. This leads to the following allowed combinations of command and state interfaces: 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 ad4abb6335..05d5fe992c 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 @@ -15,6 +15,7 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ +#include #include // for std::reference_wrapper #include #include @@ -54,14 +55,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa public: JointTrajectoryController(); - /** - * @brief command_interface_configuration - */ controller_interface::InterfaceConfiguration command_interface_configuration() const override; - /** - * @brief command_interface_configuration - */ controller_interface::InterfaceConfiguration state_interface_configuration() const override; controller_interface::return_type update( @@ -152,9 +147,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // 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_; + std::atomic rt_is_holding_{false}; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported - bool subscriber_is_active_ = false; + std::atomic subscriber_is_active_{false}; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = nullptr; @@ -171,15 +166,16 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa using StatePublisherPtr = std::unique_ptr; rclcpp::Publisher::SharedPtr publisher_; StatePublisherPtr state_publisher_; + ControllerStateMsg state_msg_; using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; - 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_action::Server::SharedPtr action_server_; + std::atomic rt_has_pending_goal_{false}; ///< Is there a pending action goal? rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); @@ -287,7 +283,15 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa { for (size_t index = 0; index < num_cmd_joints_; ++index) { - joint_interface[index].get().set_value(trajectory_point_interface[map_cmd_to_joints_[index]]); + if (!joint_interface[index].get().set_value( + trajectory_point_interface[map_cmd_to_joints_[index]])) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to set value for joint '%s' in command interface '%s'. ", + command_joint_names_[index].c_str(), joint_interface[index].get().get_name().c_str()); + return; + } } } }; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index a89991bd55..7daf6abad6 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -316,7 +316,7 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "State tolerances failed for joint %lu:", joint_idx); + RCLCPP_ERROR(logger, "State tolerances failed for joint %zu:", joint_idx); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 48f5e052d2..3db25f9f7b 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.24.0 + 4.33.1 Controller for executing joint-space trajectories on a group of joints Bence Magyar diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6890cea55e..6661bc802b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -157,8 +157,7 @@ controller_interface::return_type JointTrajectoryController::update( auto new_external_msg = new_trajectory_msg_.readFromRT(); // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) if ( - current_trajectory_msg != *new_external_msg && - (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) + current_trajectory_msg != *new_external_msg && (rt_has_pending_goal_ && !active_goal) == false) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); @@ -167,7 +166,8 @@ controller_interface::return_type JointTrajectoryController::update( } // current state update - state_current_.time_from_start.set__sec(0); + state_current_.time_from_start.sec = 0; + state_current_.time_from_start.nanosec = 0; read_state_from_state_interfaces(state_current_); // currently carrying out a trajectory @@ -203,12 +203,15 @@ controller_interface::return_type JointTrajectoryController::update( // Sample expected state from the trajectory current_trajectory_->sample( traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + state_desired_.time_from_start = traj_time_ - current_trajectory_->time_from_start(); // Sample setpoint for next control cycle const bool valid_point = current_trajectory_->sample( traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr, end_segment_itr, false); + state_current_.time_from_start = time - current_trajectory_->time_from_start(); + if (valid_point) { const rclcpp::Time traj_start = current_trajectory_->time_from_start(); @@ -229,7 +232,7 @@ controller_interface::return_type JointTrajectoryController::update( // 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 && + !before_last_point && !rt_is_holding_ && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) { RCLCPP_WARN(logger, "Aborted due to command timeout"); @@ -247,7 +250,7 @@ controller_interface::return_type JointTrajectoryController::update( // is the last point // print output per default, goal will be aborted afterwards if ( - (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && + (before_last_point || first_sample) && !rt_is_holding_ && !check_state_tolerance_per_joint( state_error_, index, active_tol->state_tolerance[index], true /* show_errors */)) { @@ -255,7 +258,7 @@ controller_interface::return_type JointTrajectoryController::update( } // past the final point, check that we end up inside goal tolerance if ( - !before_last_point && *(rt_is_holding_.readFromRT()) == false && + !before_last_point && !rt_is_holding_ && !check_state_tolerance_per_joint( state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */)) { @@ -356,7 +359,7 @@ controller_interface::return_type JointTrajectoryController::update( // 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); + rt_has_pending_goal_ = false; RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); @@ -375,7 +378,7 @@ controller_interface::return_type JointTrajectoryController::update( // 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); + rt_has_pending_goal_ = false; RCLCPP_INFO(logger, "Goal reached, success!"); @@ -394,7 +397,7 @@ controller_interface::return_type JointTrajectoryController::update( // 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); + rt_has_pending_goal_ = false; RCLCPP_WARN(logger, "%s", error_string.c_str()); @@ -403,7 +406,7 @@ controller_interface::return_type JointTrajectoryController::update( } } } - else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) + else if (tolerance_violated_while_moving && !rt_has_pending_goal_) { // 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"); @@ -411,8 +414,7 @@ controller_interface::return_type JointTrajectoryController::update( new_trajectory_msg_.reset(); new_trajectory_msg_.initRT(set_hold_position()); } - else if ( - !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) + else if (!before_last_point && !within_goal_time && !rt_has_pending_goal_) { RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position..."); @@ -431,12 +433,22 @@ controller_interface::return_type JointTrajectoryController::update( void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) { + auto logger = get_node()->get_logger(); auto assign_point_from_state_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(); + const auto joint_state_interface_value_op = joint_interface[index].get().get_optional(); + if (!joint_state_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve joint state interface value for joint at index %zu", index); + } + else + { + trajectory_point_interface[index] = joint_state_interface_value_op.value(); + } } }; auto assign_point_from_command_interface = @@ -447,8 +459,17 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory std::numeric_limits::quiet_NaN()); for (size_t index = 0; index < num_cmd_joints_; ++index) { - trajectory_point_interface[map_cmd_to_joints_[index]] = - joint_interface[index].get().get_value(); + const auto joint_command_interface_value_op = joint_interface[index].get().get_optional(); + if (!joint_command_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve joint command interface value for joint at index %zu", index); + } + else + { + trajectory_point_interface[map_cmd_to_joints_[index]] = + joint_command_interface_value_op.value(); + } } }; @@ -492,16 +513,29 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto { for (size_t index = 0; index < num_cmd_joints_; ++index) { - trajectory_point_interface[map_cmd_to_joints_[index]] = - joint_interface[index].get().get_value(); + const auto joint_interface_value_op = joint_interface[index].get().get_optional(); + if (!joint_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve value of joint interface for joint at index %zu", index); + } + else + { + trajectory_point_interface[map_cmd_to_joints_[index]] = joint_interface_value_op.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(); + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) + { + auto interface_op = interface.get().get_optional(); + return !interface_op.has_value() || std::isnan(interface_op.value()); + }) == joint_interface.end(); }; // Assign values from the command interfaces as state. Therefore needs check for both. @@ -577,16 +611,27 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( { for (size_t index = 0; index < num_cmd_joints_; ++index) { - trajectory_point_interface[map_cmd_to_joints_[index]] = - joint_interface[index].get().get_value(); + auto joint_interface_op = joint_interface[index].get().get_optional(); + if (!joint_interface_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Unable to retrieve value of joint interface at index %zu", + index); + continue; + } + trajectory_point_interface[map_cmd_to_joints_[index]] = joint_interface_op.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(); + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) + { + auto interface_op = interface.get().get_optional(); + return !interface_op.has_value() || std::isnan(interface_op.value()); + }) == joint_interface.end(); }; // Assign values from the command interfaces as command. @@ -749,7 +794,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( for (size_t i = 0; i < command_joint_names_.size(); i++) { RCLCPP_DEBUG( - logger, "Command joint %lu: '%s' maps to joint %lu: '%s'.", i, + logger, "Command joint %zu: '%s' maps to joint %zu: '%s'.", i, command_joint_names_[i].c_str(), map_cmd_to_joints_[i], params_.joints.at(map_cmd_to_joints_[i]).c_str()); } @@ -770,6 +815,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // Check if only allowed interface types are used and initialize storage to avoid memory // allocation during activation joint_command_interface_.resize(allowed_interface_types_.size()); + for (auto & itf : joint_command_interface_) + { + itf.reserve(params_.joints.size()); + } has_position_command_interface_ = contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_POSITION); @@ -807,6 +856,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // allocation during activation // Note: 'effort' storage is also here, but never used. Still, for this is OK. joint_state_interface_.resize(allowed_interface_types_.size()); + for (auto & itf : joint_state_interface_) + { + itf.reserve(params_.joints.size()); + } has_position_state_interface_ = contains_interface_type(params_.state_interfaces, hardware_interface::HW_IF_POSITION); @@ -886,42 +939,39 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "~/controller_state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); - state_publisher_->lock(); - state_publisher_->msg_.joint_names = params_.joints; - 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_); + state_msg_.joint_names = params_.joints; + state_msg_.reference.positions.resize(dof_); + state_msg_.reference.velocities.resize(dof_); + state_msg_.reference.accelerations.resize(dof_); + state_msg_.feedback.positions.resize(dof_); + state_msg_.error.positions.resize(dof_); if (has_velocity_state_interface_) { - state_publisher_->msg_.feedback.velocities.resize(dof_); - state_publisher_->msg_.error.velocities.resize(dof_); + state_msg_.feedback.velocities.resize(dof_); + state_msg_.error.velocities.resize(dof_); } if (has_acceleration_state_interface_) { - state_publisher_->msg_.feedback.accelerations.resize(dof_); - state_publisher_->msg_.error.accelerations.resize(dof_); + state_msg_.feedback.accelerations.resize(dof_); + state_msg_.error.accelerations.resize(dof_); } if (has_position_command_interface_) { - state_publisher_->msg_.output.positions.resize(dof_); + state_msg_.output.positions.resize(dof_); } if (has_velocity_command_interface_) { - state_publisher_->msg_.output.velocities.resize(dof_); + state_msg_.output.velocities.resize(dof_); } if (has_acceleration_command_interface_) { - state_publisher_->msg_.output.accelerations.resize(dof_); + state_msg_.output.accelerations.resize(dof_); } if (has_effort_command_interface_) { - state_publisher_->msg_.output.effort.resize(dof_); + state_msg_.output.effort.resize(dof_); } - state_publisher_->unlock(); - // action server configuration if (params_.allow_partial_joints_goal) { @@ -1033,11 +1083,16 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( read_state_from_state_interfaces(state_current_); read_state_from_state_interfaces(last_commanded_state_); } + // reset/zero out all of the PID's (The integral term is not retained and reset to zero) + for (auto & pid : pids_) + { + pid->reset(); + } last_commanded_time_ = rclcpp::Time(); // 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); + rt_is_holding_ = true; // parse timeout parameter if (params_.cmd_timeout > 0.0) @@ -1067,9 +1122,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( const rclcpp_lifecycle::State &) { const auto active_goal = *rt_active_goal_.readFromNonRT(); + auto logger = get_node()->get_logger(); if (active_goal) { - rt_has_pending_goal_.writeFromNonRT(false); + rt_has_pending_goal_ = 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."); @@ -1081,24 +1137,39 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( { if (has_position_command_interface_) { - joint_command_interface_[0][index].get().set_value( - joint_command_interface_[0][index].get().get_value()); + const auto joint_position_value_op = joint_command_interface_[0][index].get().get_optional(); + if (!joint_position_value_op.has_value()) + { + RCLCPP_WARN(logger, "Unable to retrieve joint position value"); + return controller_interface::CallbackReturn::SUCCESS; + } + if (!joint_command_interface_[0][index].get().set_value(joint_position_value_op.value())) + { + RCLCPP_WARN( + logger, "Unable to set the joint position to value: %f", joint_position_value_op.value()); + return controller_interface::CallbackReturn::SUCCESS; + } } - if (has_velocity_command_interface_) + if (has_velocity_command_interface_ && !joint_command_interface_[1][index].get().set_value(0.0)) { - joint_command_interface_[1][index].get().set_value(0.0); + RCLCPP_WARN(logger, "Error while setting joint velocity to value 0.0"); + return controller_interface::CallbackReturn::SUCCESS; } - if (has_acceleration_command_interface_) + if ( + has_acceleration_command_interface_ && + !joint_command_interface_[2][index].get().set_value(0.0)) { - joint_command_interface_[2][index].get().set_value(0.0); + RCLCPP_WARN(logger, "Error while setting joint acceleration to value 0.0"); + return controller_interface::CallbackReturn::SUCCESS; } // TODO(anyone): How to halt when using effort commands? - if (has_effort_command_interface_) + if (has_effort_command_interface_ && !joint_command_interface_[3][index].get().set_value(0.0)) { - joint_command_interface_[3][index].get().set_value(0.0); + RCLCPP_WARN(logger, "Error while setting joint effort to value 0.0"); + return controller_interface::CallbackReturn::SUCCESS; } } @@ -1107,7 +1178,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( joint_command_interface_[index].clear(); joint_state_interface_[index].clear(); } - release_interfaces(); subscriber_is_active_ = false; @@ -1148,30 +1218,32 @@ void JointTrajectoryController::publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) { - if (state_publisher_->trylock()) - { - state_publisher_->msg_.header.stamp = time; - 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; - state_publisher_->msg_.error.positions = state_error.positions; + if (state_publisher_) + { + state_msg_.header.stamp = time; + state_msg_.reference.time_from_start = desired_state.time_from_start; + state_msg_.reference.positions = desired_state.positions; + state_msg_.reference.velocities = desired_state.velocities; + state_msg_.reference.accelerations = desired_state.accelerations; + state_msg_.feedback.time_from_start = current_state.time_from_start; + state_msg_.feedback.positions = current_state.positions; + state_msg_.error.positions = state_error.positions; if (has_velocity_state_interface_) { - state_publisher_->msg_.feedback.velocities = current_state.velocities; - state_publisher_->msg_.error.velocities = state_error.velocities; + state_msg_.feedback.velocities = current_state.velocities; + state_msg_.error.velocities = state_error.velocities; } if (has_acceleration_state_interface_) { - state_publisher_->msg_.feedback.accelerations = current_state.accelerations; - state_publisher_->msg_.error.accelerations = state_error.accelerations; + state_msg_.feedback.accelerations = current_state.accelerations; + state_msg_.error.accelerations = state_error.accelerations; } if (read_commands_from_command_interfaces(command_current_)) { - state_publisher_->msg_.output = command_current_; + state_msg_.output = command_current_; } - state_publisher_->unlockAndPublish(); + state_publisher_->try_publish(state_msg_); } } @@ -1187,7 +1259,7 @@ void JointTrajectoryController::topic_callback( if (subscriber_is_active_) { add_new_trajectory_msg(msg); - rt_is_holding_.writeFromNonRT(false); + rt_is_holding_ = false; } }; @@ -1226,7 +1298,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback get_node()->get_logger(), "Canceling active action goal because cancel callback received."); // Mark the current goal as canceled - rt_has_pending_goal_.writeFromNonRT(false); + rt_has_pending_goal_ = false; auto action_res = std::make_shared(); active_goal->setCanceled(action_res); rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); @@ -1241,7 +1313,7 @@ void JointTrajectoryController::goal_accepted_callback( std::shared_ptr> goal_handle) { // mark a pending goal - rt_has_pending_goal_.writeFromNonRT(true); + rt_has_pending_goal_ = true; // Update new trajectory { @@ -1250,7 +1322,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); + rt_is_holding_ = false; } // Update the active goal @@ -1331,17 +1403,38 @@ void JointTrajectoryController::fill_partial_goal( // Assume hold position with 0 velocity and acceleration for missing joints if (!it.positions.empty()) { - if ( - has_position_command_interface_ && - !std::isnan(joint_command_interface_[0][index].get().get_value())) + if (has_position_command_interface_) { - // copy last command if cmd interface exists - it.positions.push_back(joint_command_interface_[0][index].get().get_value()); + const auto position_command_value_op = + joint_command_interface_[0][index].get().get_optional(); + + if (!position_command_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve position command value of joint at index %zu", index); + } + else if (!std::isnan(position_command_value_op.value())) + { + it.positions.push_back(position_command_value_op.value()); + } } + else if (has_position_state_interface_) { // copy current state if state interface exists - it.positions.push_back(joint_state_interface_[0][index].get().get_value()); + const auto position_state_value_op = + joint_state_interface_[0][index].get().get_optional(); + if (!position_state_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve position state value of joint at index %zu", index); + } + else if (!std::isnan(position_state_value_op.value())) + { + it.positions.push_back(position_state_value_op.value()); + } } } if (!it.velocities.empty()) @@ -1602,7 +1695,7 @@ JointTrajectoryController::set_hold_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); + rt_is_holding_ = true; return hold_position_msg_ptr_; } @@ -1616,7 +1709,7 @@ JointTrajectoryController::set_success_trajectory_point() 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); + rt_is_holding_ = true; return hold_position_msg_ptr_; } @@ -1674,16 +1767,24 @@ void JointTrajectoryController::update_pids() for (size_t i = 0; i < num_cmd_joints_; ++i) { const auto & gains = params_.gains.joints_map.at(params_.joints.at(map_cmd_to_joints_[i])); + control_toolbox::AntiWindupStrategy antiwindup_strat; + antiwindup_strat.set_type(gains.antiwindup_strategy); + // deprecated parameter i_clamp: i_clamp_max and i_clamp_min have precedence over i_clamp + antiwindup_strat.i_max = std::isfinite(gains.i_clamp_max) ? gains.i_clamp_max : gains.i_clamp; + antiwindup_strat.i_min = std::isfinite(gains.i_clamp_min) ? gains.i_clamp_min : -gains.i_clamp; + antiwindup_strat.error_deadband = gains.error_deadband; + antiwindup_strat.tracking_time_constant = gains.tracking_time_constant; if (pids_[i]) { // update PIDs with gains from ROS parameters - pids_[i]->set_gains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + pids_[i]->set_gains( + gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat); } 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); + gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat); } ff_velocity_scale_[i] = gains.ff_velocity_scale; } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 926366cde6..b06c8c2e26 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -132,14 +132,61 @@ joint_trajectory_controller: } i_clamp: { type: double, - default_value: 0.0, - description: "Integral clamp. Symmetrical in both positive and negative direction." + default_value: .inf, + description: "[Deprecated, use i_clamp_max/i_clamp_min] Integral clamp. Symmetrical in both positive and negative direction." + } + i_clamp_max: { + type: double, + default_value: .inf, + description: "Upper integral clamp." + } + i_clamp_min: { + type: double, + default_value: -.inf, + description: "Lower integral clamp." } ff_velocity_scale: { type: double, default_value: 0.0, description: "Feed-forward scaling :math:`k_{ff}` of velocity" } + u_clamp_max: { + type: double, + default_value: .inf, + description: "Upper output clamp." + } + u_clamp_min: { + type: double, + default_value: -.inf, + description: "Lower output clamp." + } + antiwindup_strategy: { + type: string, + default_value: "legacy", + read_only: true, + description: "Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', 'legacy' or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior.", + validation: { + one_of<>: [[ + "back_calculation", + "conditional_integration", + "legacy", + "none" + ]] + } + } + tracking_time_constant: { + type: double, + default_value: 0.0, + description: "Specifies the tracking time constant for the 'back_calculation' strategy. If + set to 0.0 when this strategy is selected, a recommended default value will be applied." + } + error_deadband: { + type: double, + default_value: 0.0, + description: "Is used to stop integration when the error is within the given range." + } constraints: stopped_velocity_tolerance: { type: double, diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 3ced840c02..86282e875e 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -177,6 +177,7 @@ bool Trajectory::sample( } start_segment_itr = begin() + static_cast(i); end_segment_itr = begin() + static_cast(i + 1); + output_state.time_from_start = next_point.time_from_start; if (search_monotonically_increasing) { last_sample_idx_ = i; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index ec3391a9e1..56ec6aecd2 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -163,10 +163,9 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) traj_controller_->update( rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); - auto state = traj_controller_->get_node()->deactivate(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + DeactivateTrajectoryController(); - state = traj_controller_->get_node()->cleanup(); + auto state = traj_controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); executor.cancel(); @@ -192,6 +191,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param { rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); + SetPidParameters(0.0, 1.0); traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); traj_controller_->get_node()->set_parameter(rclcpp::Parameter("update_rate", 10)); @@ -237,8 +237,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // 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); + DeactivateTrajectoryController(); // it should be holding the current point expectHoldingPointDeactivated(deactivated_positions); @@ -462,6 +461,33 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_jo } } +TEST_F(TrajectoryControllerTest, time_from_start_populated) +{ + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {}); + subscribeToState(executor); + + // schedule a single waypoint at 100ms: + builtin_interfaces::msg::Duration tfs; + tfs.sec = 0; + tfs.nanosec = 100000000u; + publish(tfs, {INITIAL_POS_JOINTS}, rclcpp::Time(0)); + traj_controller_->wait_for_trajectory(executor); + + // update for 0.2s + updateController(rclcpp::Duration::from_seconds(0.2)); + // give the publish timer one more spin + executor.spin_some(); + + auto state = getState(); + ASSERT_TRUE(state); + // should be around 0.2s + EXPECT_EQ(state->feedback.time_from_start.sec, 0u); + EXPECT_NEAR(state->feedback.time_from_start.nanosec, 200000000u, 10000000u); + EXPECT_EQ(state->reference.time_from_start.sec, 0u); + EXPECT_NEAR(state->reference.time_from_start.nanosec, 200000000u, 10000000u); +} + /** * @brief check if dynamic parameters are updated */ @@ -585,8 +611,6 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru 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}}}; @@ -677,8 +701,6 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal 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}}}; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f95ef24c8e..b05c7a8116 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -206,11 +206,6 @@ class TestableJointTrajectoryController trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } trajectory_msgs::msg::JointTrajectoryPoint get_current_command() { return command_current_; } - control_msgs::msg::JointTrajectoryControllerState get_state_msg() - { - return state_publisher_->msg_; - } - /** * a copy of the private member function */ @@ -236,7 +231,7 @@ class TrajectoryControllerTest : public ::testing::Test public: static void SetUpTestCase() { rclcpp::init(0, nullptr); } - virtual void SetUp() + void SetUp() override { controller_name_ = "test_joint_trajectory_controller"; @@ -284,11 +279,18 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_ = std::make_shared(); auto node_options = rclcpp::NodeOptions(); + // read-only parameters have to be set before init 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_)); + // avoid deprecation warning for legacy (default) antiwindup strategy + for (const auto & joint : joint_names_) + { + parameter_overrides.push_back( + rclcpp::Parameter("gains." + joint + ".antiwindup_strategy", "none")); + } parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); traj_controller_->set_node_options(node_options); @@ -308,7 +310,7 @@ class TrajectoryControllerTest : public ::testing::Test 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 i_clamp(prefix + ".i_clamp", 1000.0); const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value); node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); } @@ -393,13 +395,13 @@ class TrajectoryControllerTest : public ::testing::Test // 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]); + (void)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]); + (void)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]); + (void)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]); + (void)cmd_interfaces.back().set_value(initial_eff_joints[i]); if (separate_cmd_and_state_values) { joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; @@ -426,6 +428,7 @@ class TrajectoryControllerTest : public ::testing::Test EXPECT_EQ( traj_controller_->get_node()->deactivate().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + traj_controller_->release_interfaces(); } } } @@ -689,9 +692,10 @@ class TrajectoryControllerTest : public ::testing::Test 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])) + position.at(i) - pos_state_interfaces_[i].get_optional().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]; + << pos_state_interfaces_[i].get_optional().value() << ", velocity command is " + << joint_vel_[i]; } } if (traj_controller_->has_effort_command_interface()) @@ -699,9 +703,11 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < 3; i++) { EXPECT_TRUE(is_same_sign_or_zero( - position.at(i) - pos_state_interfaces_[i].get_value() + effort.at(i), joint_eff_[i])) + position.at(i) - pos_state_interfaces_[i].get_optional().value() + effort.at(i), + joint_eff_[i])) << "test position point " << position.at(i) << ", position state is " - << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; + << pos_state_interfaces_[i].get_optional().value() << ", effort command is " + << joint_eff_[i]; } } } diff --git a/mecanum_drive_controller/CHANGELOG.rst b/mecanum_drive_controller/CHANGELOG.rst index a28632e131..b88285e046 100644 --- a/mecanum_drive_controller/CHANGELOG.rst +++ b/mecanum_drive_controller/CHANGELOG.rst @@ -2,6 +2,56 @@ Changelog for package mecanum_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* mecanum_drive_controller: Declare missing backward_ros dependency (backport `#1941 `_) (`#1943 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- +* Mecanum Drive: Populate the pose covariance matrix (backport `#1772 `_) (`#1807 `_) +* Add tf_frame_prefix parameters to mecanum_drive_controller (backport `#1680 `_) (`#1810 `_) +* Contributors: Hilary Luo, Dawid Kmak + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Add missing github_url to rst files (backport `#1717 `_) (`#1719 `_) +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- +* Simplify `on_set_chained_mode` avoiding cpplint warnings (backport `#1564 `_) (`#1688 `_) +* Deprecating tf2 C Headers (`#1325 `_) +* Contributors: Lucas Wendland, mergify[bot], Bhagyesh Agresar + 4.24.0 (2025-04-27) ------------------- * Fix preceeding->preceding typos (`#1655 `_) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 5e9d2227b6..3755d86e17 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -42,8 +42,18 @@ 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}) + mecanum_drive_controller_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + tf2::tf2 + ${tf2_geometry_msgs_TARGETS} + ${tf2_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${std_srvs_TARGETS}) pluginlib_export_plugin_description_file( controller_interface mecanum_drive_controller.xml) @@ -57,10 +67,10 @@ if(BUILD_TESTING) 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 + target_link_libraries(test_load_mecanum_drive_controller + controller_manager::controller_manager + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets ) add_rostest_with_parameters_gmock( @@ -68,22 +78,12 @@ if(BUILD_TESTING) ${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_preceding test/test_mecanum_drive_controller_preceding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/mecanum_drive_controller_preceding_params.yaml) target_include_directories(test_mecanum_drive_controller_preceding PRIVATE include) target_link_libraries(test_mecanum_drive_controller_preceding mecanum_drive_controller) - ament_target_dependencies( - test_mecanum_drive_controller_preceding - controller_interface - hardware_interface - ) endif() install( diff --git a/mecanum_drive_controller/doc/userdoc.rst b/mecanum_drive_controller/doc/userdoc.rst index e22bcb6cdc..d823281110 100644 --- a/mecanum_drive_controller/doc/userdoc.rst +++ b/mecanum_drive_controller/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/mecanum_drive_controller/doc/userdoc.rst + .. _mecanum_drive_controller_userdoc: mecanum_drive_controller 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 index 189805c17c..58b6de04b7 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -29,8 +29,8 @@ #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 "realtime_tools/realtime_thread_safe_box.hpp" #include "std_srvs/srv/set_bool.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -109,26 +109,30 @@ class MecanumDriveController : public controller_interface::ChainableControllerI */ 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_; + // the RT Box containing the command message + realtime_tools::RealtimeThreadSafeBox input_ref_; + // save the last reference in case of unable to get value from box + ControllerReferenceMsg current_ref_; + // the reference timeout value from parameters + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // Command subscribers and Controller State, odom state, tf state publishers rclcpp::Subscription::SharedPtr ref_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_; + OdomStateMsg odom_state_msg_; using TfStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; std::unique_ptr rt_tf_odom_state_publisher_; + TfStateMsg tf_odom_state_msg_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr controller_s_publisher_; std::unique_ptr controller_state_publisher_; + ControllerStateMsg controller_state_msg_; // override methods from ChainableControllerInterface std::vector on_export_reference_interfaces() override; diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp index 79a1b54846..823764ccfe 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/odometry.hpp @@ -15,9 +15,10 @@ #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" +#include +#include + +#include "rclcpp/time.hpp" #define PLANAR_POINT_DIM 3 diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index 847e8affcd..8f828d9476 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -2,7 +2,7 @@ mecanum_drive_controller - 4.24.0 + 4.33.1 Implementation of mecanum drive controller for 4 wheel drive. @@ -25,6 +25,7 @@ generate_parameter_library ros2_control_cmake + backward_ros control_msgs controller_interface geometry_msgs diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 3ec118bf8c..e468c8577b 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -33,16 +33,15 @@ using ControllerReferenceMsg = // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, - const std::shared_ptr & node) + ControllerReferenceMsg & 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(); + 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 @@ -129,9 +128,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( "~/reference", subscribers_qos, std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1)); - std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.set(current_ref_); try { @@ -148,21 +146,41 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( 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; + // Append the tf prefix if there is one + std::string tf_prefix = ""; + if (params_.tf_frame_prefix_enable) + { + tf_prefix = params_.tf_frame_prefix != "" ? params_.tf_frame_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 = tf_prefix + params_.odom_frame_id; + const auto base_frame_id = tf_prefix + params_.base_frame_id; + + odom_state_msg_.header.stamp = get_node()->now(); + odom_state_msg_.header.frame_id = odom_frame_id; + odom_state_msg_.child_frame_id = base_frame_id; + odom_state_msg_.pose.pose.position.z = 0; - auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + auto & pose_covariance = odom_state_msg_.pose.covariance; + auto & twist_covariance = odom_state_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]; + pose_covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + twist_covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } - rt_odom_state_publisher_->unlock(); try { @@ -179,13 +197,11 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( 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(); + tf_odom_state_msg_.transforms.resize(1); + tf_odom_state_msg_.transforms[0].header.stamp = get_node()->now(); + tf_odom_state_msg_.transforms[0].header.frame_id = odom_frame_id; + tf_odom_state_msg_.transforms[0].child_frame_id = base_frame_id; + tf_odom_state_msg_.transforms[0].transform.translation.z = 0.0; try { @@ -205,10 +221,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( 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(); + controller_state_msg_.header.stamp = get_node()->now(); + controller_state_msg_.header.frame_id = odom_frame_id; RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); @@ -231,7 +245,7 @@ void MecanumDriveController::reference_callback(const std::shared_ptrheader.stamp).seconds(), age_of_last_command.seconds(), ref_timeout_.seconds()); - reset_controller_reference_msg(msg, get_node()); + ControllerReferenceMsg emtpy_msg; + reset_controller_reference_msg(emtpy_msg, get_node()); + input_ref_.set(emtpy_msg); } } @@ -298,17 +314,16 @@ MecanumDriveController::on_export_reference_interfaces() return reference_interfaces; } -bool MecanumDriveController::on_set_chained_mode(bool chained_mode) -{ - // Always accept switch to/from chained mode - return true || chained_mode; -} +bool MecanumDriveController::on_set_chained_mode(bool /*chained_mode*/) { return true; } 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()); + // Try to set default value in command. + // If this fails, then another command will be received soon anyways. + ControllerReferenceMsg emtpy_msg; + reset_controller_reference_msg(emtpy_msg, get_node()); + input_ref_.try_set(emtpy_msg); return controller_interface::CallbackReturn::SUCCESS; } @@ -337,41 +352,50 @@ controller_interface::CallbackReturn MecanumDriveController::on_deactivate( controller_interface::return_type MecanumDriveController::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - (current_ref)->header.stamp; + auto current_ref_op = input_ref_.try_get(); + if (current_ref_op.has_value()) + { + current_ref_ = current_ref_op.value(); + } + + const auto age_of_last_command = time - current_ref_.header.stamp; - // send message only if there is no timeout + // accept 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)) + !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; + 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(); + 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(); + + input_ref_.try_set(current_ref_); } } } else { if ( - !std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.linear.y) && - !std::isnan(current_ref->twist.angular.z)) + !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(); + 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(); + + input_ref_.try_set(current_ref_); } } @@ -382,10 +406,26 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma 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(); + const auto wheel_front_left_state_vel_op = state_interfaces_[FRONT_LEFT].get_optional(); + const auto wheel_front_right_state_vel_op = state_interfaces_[FRONT_RIGHT].get_optional(); + const auto wheel_rear_right_state_vel_op = state_interfaces_[REAR_RIGHT].get_optional(); + const auto wheel_rear_left_state_vel_op = state_interfaces_[REAR_LEFT].get_optional(); + + if ( + !wheel_front_left_state_vel_op.has_value() || !wheel_front_right_state_vel_op.has_value() || + !wheel_rear_right_state_vel_op.has_value() || !wheel_rear_left_state_vel_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Unable to retrieve data from front left wheel or front right wheel or rear left wheel or " + "rear right wheel"); + return controller_interface::return_type::OK; + } + + const double wheel_front_left_state_vel = wheel_front_left_state_vel_op.value(); + const double wheel_front_right_state_vel = wheel_front_right_state_vel_op.value(); + const double wheel_rear_right_state_vel = wheel_rear_right_state_vel_op.value(); + const double wheel_rear_left_state_vel = wheel_rear_left_state_vel_op.value(); if ( !std::isnan(wheel_front_left_state_vel) && !std::isnan(wheel_rear_left_state_vel) && @@ -478,44 +518,41 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma orientation.setRPY(0.0, 0.0, odometry_.getRz()); // Populate odom message and publish - if (rt_odom_state_publisher_->trylock()) + if (rt_odom_state_publisher_) { - 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(); + odom_state_msg_.header.stamp = time; + odom_state_msg_.pose.pose.position.x = odometry_.getX(); + odom_state_msg_.pose.pose.position.y = odometry_.getY(); + odom_state_msg_.pose.pose.orientation = tf2::toMsg(orientation); + odom_state_msg_.twist.twist.linear.x = odometry_.getVx(); + odom_state_msg_.twist.twist.linear.y = odometry_.getVy(); + odom_state_msg_.twist.twist.angular.z = odometry_.getWz(); + rt_odom_state_publisher_->try_publish(odom_state_msg_); } // Publish tf /odom frame - if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_) { - 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(); + tf_odom_state_msg_.transforms.front().header.stamp = time; + tf_odom_state_msg_.transforms.front().transform.translation.x = odometry_.getX(); + tf_odom_state_msg_.transforms.front().transform.translation.y = odometry_.getY(); + tf_odom_state_msg_.transforms.front().transform.rotation = tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->try_publish(tf_odom_state_msg_); } - if (controller_state_publisher_->trylock()) + if (controller_state_publisher_) { - 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(); + controller_state_msg_.header.stamp = get_node()->now(); + + controller_state_msg_.front_left_wheel_velocity = wheel_front_left_state_vel; + controller_state_msg_.front_right_wheel_velocity = wheel_front_right_state_vel; + controller_state_msg_.back_right_wheel_velocity = wheel_rear_right_state_vel; + controller_state_msg_.back_left_wheel_velocity = wheel_rear_left_state_vel; + + controller_state_msg_.reference_velocity.linear.x = reference_interfaces_[0]; + controller_state_msg_.reference_velocity.linear.y = reference_interfaces_[1]; + controller_state_msg_.reference_velocity.angular.z = reference_interfaces_[2]; + controller_state_publisher_->try_publish(controller_state_msg_); } reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 896b731953..058270fd7a 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -113,12 +113,21 @@ mecanum_drive_controller: read_only: false, } + tf_frame_prefix_enable: { + type: bool, + default_value: true, + description: "Enables or disables appending ``tf_frame_prefix`` to tf frame id's. See its parameter description for more information.", + } + tf_frame_prefix: { + type: string, + default_value: "", + description: "(optional) Prefix to be appended to the tf frames, will be added to odom_frame_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + } 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, diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index c764c6c063..dfa3e5de94 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -1,40 +1,41 @@ -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] +/**: + 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, 6.0, 12.0, 18.0, 24.0, 30.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_preceding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceding_params.yaml index c4e5bb391a..72030202b9 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_preceding_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceding_params.yaml @@ -1,24 +1,25 @@ -test_mecanum_drive_controller: - ros__parameters: - reference_timeout: 0.1 +/**: + 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_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" + 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 + 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] + 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_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 00c1f5a74b..a1ec5fdba9 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -62,6 +62,13 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para 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_.pose_covariance_diagonal, + std::vector({0.0, 6.0, 12.0, 18.0, 24.0, 30.0})); + ASSERT_EQ( + controller_->params_.twist_covariance_diagonal, + std::vector({0.0, 7.0, 14.0, 21.0, 28.0, 35.0})); + ASSERT_EQ( controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); ASSERT_EQ( @@ -118,6 +125,177 @@ TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_ex } } +TEST_F(MecanumDriveControllerTest, 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"; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(false)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->odom_state_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); + + std::array pose_covariance = { + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 6.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 12.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 18.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 24.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 30.0}}; + + std::array twist_covariance = { + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 7.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 14.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 21.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 28.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 35.0}}; + + ASSERT_EQ(odometry_message.pose.covariance, pose_covariance); + ASSERT_EQ(odometry_message.twist.covariance, twist_covariance); +} + +TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->odom_state_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(MecanumDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->odom_state_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(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_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"; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(false)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options, test_namespace); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->odom_state_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(MecanumDriveControllerTest, 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"; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options, test_namespace); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->odom_state_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(MecanumDriveControllerTest, 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 = ""; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options, test_namespace); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->odom_state_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); +} + TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset) { SetUpController(); @@ -126,10 +304,9 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference 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)); + auto msg = controller_->input_ref_.get(); + 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) @@ -161,11 +338,11 @@ TEST_F( 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_->command_interfaces_[NR_CMD_ITFS - 4].get_optional().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_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_optional().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_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_optional().value())); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -247,11 +424,11 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re 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)); + auto reference = controller_->input_ref_.get(); + 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) @@ -260,10 +437,10 @@ TEST_F(MecanumDriveControllerTest, when_reference_msg_is_too_old_expect_unset_re 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)); + ASSERT_EQ(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)); } // when time stamp is zero expect that time stamp is set to current time stamp @@ -278,11 +455,11 @@ TEST_F( 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)); + auto reference = controller_->input_ref_.get(); + 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) @@ -290,13 +467,15 @@ TEST_F( 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); + reference = controller_->input_ref_.get(); + + ASSERT_EQ(old_timestamp.sec, reference.header.stamp.sec); + EXPECT_FALSE(std::isnan(reference.twist.linear.x)); + EXPECT_FALSE(std::isnan(reference.twist.angular.z)); + EXPECT_EQ(reference.twist.linear.x, 1.5); + EXPECT_EQ(reference.twist.linear.y, 0.0); + EXPECT_EQ(reference.twist.angular.z, 0.0); + EXPECT_NE(reference.header.stamp.sec, 0.0); } // when the reference_msg has valid timestamp then the timeout check in reference_callback() @@ -311,9 +490,9 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer 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)); + auto reference = controller_->input_ref_.get(); + 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) @@ -321,11 +500,12 @@ TEST_F(MecanumDriveControllerTest, when_message_has_valid_timestamp_expect_refer 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); + reference = controller_->input_ref_.get(); + EXPECT_FALSE(std::isnan(reference.twist.linear.x)); + EXPECT_FALSE(std::isnan(reference.twist.angular.z)); + EXPECT_EQ(reference.twist.linear.x, 1.5); + EXPECT_EQ(reference.twist.linear.y, 0.0); + EXPECT_EQ(reference.twist.angular.z, 0.0); } // when not in chainable mode and ref_msg_timedout expect @@ -354,23 +534,24 @@ TEST_F( // set command statically joint_command_values_[controller_->get_rear_left_wheel_index()] = command_lin_x; - std::shared_ptr msg = std::make_shared(); + ControllerReferenceMsg msg, msg_2; + + 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_.set(msg); - 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; + auto reference = controller_->input_ref_.get(); + const auto age_of_last_command = controller_->get_node()->now() - reference.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(reference.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); @@ -383,24 +564,24 @@ TEST_F( } for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_optional().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; + 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_.set(msg_2); + + reference = controller_->input_ref_.get(); + const auto age_of_last_command_2 = controller_->get_node()->now() - reference.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(reference.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); @@ -414,7 +595,7 @@ TEST_F( // 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); + ASSERT_EQ(reference.twist.linear.x, TEST_LINEAR_VELOCITY_X); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -422,7 +603,7 @@ TEST_F( for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_optional().value(), 3.0); } } @@ -481,7 +662,7 @@ TEST_F( } for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 6.0); + EXPECT_EQ(controller_->command_interfaces_[i].get_optional().value(), 6.0); } } @@ -501,21 +682,22 @@ TEST_F( 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; + ControllerReferenceMsg msg; + + 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_.set(msg); + auto reference = controller_->input_ref_.get(); + + const auto age_of_last_command = controller_->get_node()->now() - reference.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(reference.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); @@ -529,7 +711,8 @@ TEST_F( // 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)); + reference = controller_->input_ref_.get(); + ASSERT_TRUE(std::isnan(reference.twist.linear.x)); } TEST_F( @@ -542,9 +725,10 @@ TEST_F( 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)); + auto reference = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(reference.twist.linear.x)); + EXPECT_TRUE(std::isnan(reference.twist.linear.y)); + EXPECT_TRUE(std::isnan(reference.twist.angular.z)); controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // reference_callback() is called implicitly when publish_commands() is called. @@ -553,13 +737,14 @@ TEST_F( 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); + reference = controller_->input_ref_.get(); + + EXPECT_FALSE(std::isnan(reference.twist.linear.x)); + EXPECT_FALSE(std::isnan(reference.twist.linear.y)); + EXPECT_FALSE(std::isnan(reference.twist.angular.z)); + EXPECT_EQ(reference.twist.linear.x, 1.5); + EXPECT_EQ(reference.twist.linear.y, 0.0); + EXPECT_EQ(reference.twist.angular.z, 0.0); } TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest) @@ -632,10 +817,14 @@ TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest) 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(); + joint_state_values_[fl_index] = + controller_->command_interfaces_[fl_index].get_optional().value(); + joint_state_values_[fr_index] = + controller_->command_interfaces_[fr_index].get_optional().value(); + joint_state_values_[rl_index] = + controller_->command_interfaces_[rl_index].get_optional().value(); + joint_state_values_[rr_index] = + controller_->command_interfaces_[rr_index].get_optional().value(); } RCLCPP_INFO( diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 7f87824262..4d07dee7b2 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -84,6 +84,13 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once); FRIEND_TEST(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_namespace); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_namespace); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_set_namespace); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_set_namespace); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_blank_prefix_true_set_namespace); + public: controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override @@ -161,12 +168,14 @@ class MecanumDriveControllerFixture : public ::testing::Test void TearDown() { controller_.reset(nullptr); } protected: - void SetUpController(const std::string controller_name = "test_mecanum_drive_controller") + void SetUpController( + const std::string controller_name = "test_mecanum_drive_controller", + const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions(), const std::string ns = "") { const auto urdf = ""; - const auto ns = ""; + ASSERT_EQ( - controller_->init(controller_name, urdf, 0, ns, controller_->define_custom_node_options()), + controller_->init(controller_name, urdf, 0, ns, node_options), controller_interface::return_type::OK); std::vector command_ifs; diff --git a/omni_wheel_drive_controller/CHANGELOG.rst b/omni_wheel_drive_controller/CHANGELOG.rst new file mode 100644 index 0000000000..4aaec0deb5 --- /dev/null +++ b/omni_wheel_drive_controller/CHANGELOG.rst @@ -0,0 +1,274 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package omni_wheel_drive_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Fix: Remove deprecated `rclcpp::spin_some(node)` (backport `#1928 `_) (`#1932 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- +* Fix use of M_PI in omni_wheel_driver_controller on MSVC (backport `#1886 `_) (`#1887 `_) +* [omni_wheel_drive_controller] Fix the broken links (backport `#1882 `_) (`#1883 `_) +* Contributors: mergify[bot] + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- +* Add omni_wheel_drive_controller (backport `#1535 `_) (`#1836 `_) +* Contributors: mergify[bot] + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- + +4.25.0 (2025-05-17) +------------------- + +4.24.0 (2025-04-27) +------------------- + +4.23.0 (2025-04-10) +------------------- + +4.22.0 (2025-03-17) +------------------- + +4.21.0 (2025-03-01) +------------------- + +4.20.0 (2025-01-29) +------------------- + +4.19.0 (2025-01-13) +------------------- + +4.18.0 (2024-12-19) +------------------- + +4.17.0 (2024-12-07) +------------------- + +4.16.0 (2024-11-08) +------------------- + +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.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/omni_wheel_drive_controller/CMakeLists.txt b/omni_wheel_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..f3da519c2b --- /dev/null +++ b/omni_wheel_drive_controller/CMakeLists.txt @@ -0,0 +1,92 @@ +cmake_minimum_required(VERSION 3.16) +project(omni_wheel_drive_controller) + +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() + +# Find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + ament_cmake + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + Eigen3 +) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library( + ${PROJECT_NAME}_parameters + src/${PROJECT_NAME}_parameters.yaml +) + +add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}.cpp + src/odometry.cpp +) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} + PUBLIC $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + ${PROJECT_NAME}_parameters + Eigen3::Eigen + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + tf2::tf2 + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${tf2_msgs_TARGETS} +) +pluginlib_export_plugin_description_file(controller_interface omni_wheel_drive_plugin.xml) + +# Install +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}/ +) +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_parameters + EXPORT export_${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_${PROJECT_NAME} test/test_${PROJECT_NAME}.cpp) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_${PROJECT_NAME} test/test_load_${PROJECT_NAME}.cpp) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + target_link_libraries(test_load_${PROJECT_NAME} + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) +endif() + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/omni_wheel_drive_controller/doc/userdoc.rst b/omni_wheel_drive_controller/doc/userdoc.rst new file mode 100644 index 0000000000..3665d808f9 --- /dev/null +++ b/omni_wheel_drive_controller/doc/userdoc.rst @@ -0,0 +1,78 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/omni_wheel_drive_controller/doc/userdoc.rst + +.. _omni_wheel_drive_controller_userdoc: + +omni_wheel_drive_controller +================================= + +Controller for mobile robots with omnidirectional drive. + +Supports using three or more omni wheels spaced at an equal angle from each other in a circular formation. +To better understand this, have a look at :ref:`mobile_robot_kinematics`. + +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. + +Odometry is computed from hardware feedback and published. + +Other features +-------------- + + + Realtime-safe implementation. + + Odometry publishing + + Automatic stop after command time-out + +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`` double, in m/s +- ``/linear/y/velocity`` double, in m/s +- ``/angular/z/velocity`` double, in rad/s + +Together, these represent the body twist (which in unchained-mode would be obtained from ~/cmd_vel). + +State interfaces +,,,,,,,,,,,,,,,, + +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. + +Command interfaces +,,,,,,,,,,,,,,,,,,,,,, + +Joints' velocity (``hardware_interface::HW_IF_VELOCITY``) are used. + + +ROS 2 Interfaces +---------------- + +Subscribers +,,,,,,,,,,, + +~/cmd_vel [geometry_msgs/msg/TwistStamped] + Velocity command for the controller. The controller extracts the x and y component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + +Publishers +,,,,,,,,,, +~/odom [nav_msgs::msg::Odometry] + This represents an estimate of the robot's position and velocity in free space. + +/tf [tf2_msgs::msg::TFMessage] + tf tree. Published only if ``enable_odom_tf=true`` + + +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/omni_wheel_drive_controller_parameters.yaml + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/config/test_omni_wheel_drive_controller.yaml + :language: yaml diff --git a/omni_wheel_drive_controller/include/omni_wheel_drive_controller/odometry.hpp b/omni_wheel_drive_controller/include/omni_wheel_drive_controller/odometry.hpp new file mode 100644 index 0000000000..e522662596 --- /dev/null +++ b/omni_wheel_drive_controller/include/omni_wheel_drive_controller/odometry.hpp @@ -0,0 +1,76 @@ +// Copyright 2025 Aarav Gupta +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_ +#define OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_ + +#include +#include + +#include "rclcpp/time.hpp" + +namespace omni_wheel_drive_controller +{ +class Odometry +{ +public: + Odometry(); + + bool updateFromPos(const std::vector & wheels_pos, const rclcpp::Time & time); + bool updateFromVel(const std::vector & wheels_vel, const rclcpp::Time & time); + bool updateOpenLoop( + const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel, + const rclcpp::Time & time); + void resetOdometry(); + + double getX() const { return x_; } + double getY() const { return y_; } + double getHeading() const { return heading_; } + double getLinearXVel() const { return linear_x_vel_; } + double getLinearYVel() const { return linear_y_vel_; } + double getAngularVel() const { return angular_vel_; } + + void setParams( + const double & robot_radius, const double & wheel_radius, const double & wheel_offset, + const size_t & wheel_count); + +private: + Eigen::Vector3d compute_robot_velocity(const std::vector & wheels_vel) const; + void integrate(const double & dx, const double & dy, const double & dheading); + + // Current timestamp: + rclcpp::Time timestamp_; + + // Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rads] + + // Current velocity: + double linear_x_vel_; // [m/s] + double linear_y_vel_; // [m/s] + double angular_vel_; // [rads/s] + + // Robot kinematic parameters: + double robot_radius_; // [m] + double wheel_radius_; // [m] + double wheel_offset_; // [rads] + + // Previous wheel positions/states [rads]: + std::vector wheels_old_pos_; +}; + +} // namespace omni_wheel_drive_controller + +#endif // OMNI_WHEEL_DRIVE_CONTROLLER__ODOMETRY_HPP_ diff --git a/omni_wheel_drive_controller/include/omni_wheel_drive_controller/omni_wheel_drive_controller.hpp b/omni_wheel_drive_controller/include/omni_wheel_drive_controller/omni_wheel_drive_controller.hpp new file mode 100644 index 0000000000..db6f05ec93 --- /dev/null +++ b/omni_wheel_drive_controller/include/omni_wheel_drive_controller/omni_wheel_drive_controller.hpp @@ -0,0 +1,120 @@ +// Copyright 2025 Aarav Gupta +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OMNI_WHEEL_DRIVE_CONTROLLER__OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ +#define OMNI_WHEEL_DRIVE_CONTROLLER__OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "omni_wheel_drive_controller/odometry.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +// auto-generated by generate_parameter_library +#include "omni_wheel_drive_controller/omni_wheel_drive_controller_parameters.hpp" + +namespace omni_wheel_drive_controller +{ +class OmniWheelDriveController : public controller_interface::ChainableControllerInterface +{ + using TwistStamped = geometry_msgs::msg::TwistStamped; + +public: + OmniWheelDriveController(); + + controller_interface::CallbackReturn on_init() override; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + +protected: + std::vector on_export_reference_interfaces() override; + + // Parameters from ROS for omni_wheel_drive_controller + std::shared_ptr param_listener_; + Params params_; + + struct WheelHandle + { + std::optional> feedback; + std::reference_wrapper velocity; + }; + std::vector registered_wheel_handles_; + + controller_interface::CallbackReturn configure_wheel_handles( + const std::vector & wheel_names, std::vector & registered_handles); + + // Timeout to consider cmd_vel commands old + rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5); + + std::atomic_bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + // Realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox received_velocity_msg_; + // Save the last reference in case of unable to get value from box + TwistStamped command_msg_; + + std::shared_ptr> odometry_publisher_ = nullptr; + std::shared_ptr> + realtime_odometry_publisher_; + nav_msgs::msg::Odometry odometry_message_; + + Odometry odometry_; + + std::shared_ptr> odometry_transform_publisher_ = + nullptr; + std::shared_ptr> + realtime_odometry_transform_publisher_; + geometry_msgs::msg::TransformStamped transform_; + + void compute_and_set_wheel_velocities(); + const char * feedback_type() const; + void halt(); + bool reset(); + bool on_set_chained_mode(bool chained_mode) override; + +private: + void reset_buffers(); +}; +} // namespace omni_wheel_drive_controller + +#endif // OMNI_WHEEL_DRIVE_CONTROLLER__OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ diff --git a/omni_wheel_drive_controller/omni_wheel_drive_plugin.xml b/omni_wheel_drive_controller/omni_wheel_drive_plugin.xml new file mode 100644 index 0000000000..f801210d55 --- /dev/null +++ b/omni_wheel_drive_controller/omni_wheel_drive_plugin.xml @@ -0,0 +1,9 @@ + + + + A chainable controller for mobile robots with omnidirectional drive with three or more omni wheels. + + + diff --git a/omni_wheel_drive_controller/package.xml b/omni_wheel_drive_controller/package.xml new file mode 100644 index 0000000000..1724f044b6 --- /dev/null +++ b/omni_wheel_drive_controller/package.xml @@ -0,0 +1,43 @@ + + + + omni_wheel_drive_controller + 4.33.1 + A chainable controller for mobile robots with omnidirectional drive with three or + more omni wheels. + Apache License 2.0 + + Aarav Gupta + Bence Magyar + Christoph Froehlich + Denis Štogl + Sai Kishor Kothakota + + Aarav Gupta + + ament_cmake + + generate_parameter_library + ros2_control_cmake + + controller_interface + eigen + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/omni_wheel_drive_controller/src/odometry.cpp b/omni_wheel_drive_controller/src/odometry.cpp new file mode 100644 index 0000000000..0ec266a6af --- /dev/null +++ b/omni_wheel_drive_controller/src/odometry.cpp @@ -0,0 +1,162 @@ +// Copyright 2025 Aarav Gupta +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#define _USE_MATH_DEFINES + +#include "omni_wheel_drive_controller/odometry.hpp" + +#include + +namespace omni_wheel_drive_controller +{ +Odometry::Odometry() +: timestamp_(0.0), + x_(0.0), + y_(0.0), + heading_(0.0), + linear_x_vel_(0.0), + linear_y_vel_(0.0), + angular_vel_(0.0), + robot_radius_(0.0), + wheel_radius_(0.0), + wheels_old_pos_(0.0) +{ +} + +bool Odometry::updateFromPos(const std::vector & wheels_pos, const rclcpp::Time & time) +{ + const double dt = time.seconds() - timestamp_.seconds(); + // We cannot estimate angular velocity with very small time intervals + if (std::fabs(dt) < 1e-6) + { + return false; + } + + // Estimate angular velocity of wheels using old and current position [rads/s]: + std::vector wheels_vel(wheels_pos.size()); + for (size_t i = 0; i < static_cast(wheels_pos.size()); ++i) + { + wheels_vel[i] = (wheels_pos[i] - wheels_old_pos_[i]) / dt; + wheels_old_pos_[i] = wheels_pos[i]; + } + + if (updateFromVel(wheels_vel, time)) + { + return true; + } + return false; +} + +bool Odometry::updateFromVel(const std::vector & wheels_vel, const rclcpp::Time & time) +{ + const double dt = time.seconds() - timestamp_.seconds(); + + // Compute linear and angular velocities of the robot: + const Eigen::Vector3d robot_velocity = compute_robot_velocity(wheels_vel); + + // Integrate odometry: + integrate(robot_velocity(0) * dt, robot_velocity(1) * dt, robot_velocity(2) * dt); + + timestamp_ = time; + + linear_x_vel_ = robot_velocity(0); + linear_y_vel_ = robot_velocity(1); + angular_vel_ = robot_velocity(2); + + return true; +} + +Eigen::Vector3d Odometry::compute_robot_velocity(const std::vector & wheels_vel) const +{ + Eigen::MatrixXd A(wheels_vel.size(), 3); // Transformation Matrix + Eigen::VectorXd omega(wheels_vel.size()); // Wheel angular velocities vector + + const double angle_bw_wheels = (2 * M_PI) / static_cast(wheels_vel.size()); + + for (size_t i = 0; i < wheels_vel.size(); ++i) + { + // Define the transformation matrix + const double theta = (angle_bw_wheels * static_cast(i)) + wheel_offset_; + A(static_cast(i), 0) = std::sin(theta); + A(static_cast(i), 1) = -std::cos(theta); + A(static_cast(i), 2) = -robot_radius_; + + // Define the wheel angular velocities vector + omega(static_cast(i)) = wheels_vel[i]; + } + + // Compute the robot velocities using SVD decomposition + const Eigen::Vector3d V = + A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(omega * wheel_radius_); + + return V; +} + +bool Odometry::updateOpenLoop( + const double & linear_x_vel, const double & linear_y_vel, const double & angular_vel, + const rclcpp::Time & time) +{ + const double dt = time.seconds() - timestamp_.seconds(); + + // Integrate odometry: + integrate(linear_x_vel * dt, linear_y_vel * dt, angular_vel * dt); + + timestamp_ = time; + + // Save last linear and angular velocity: + linear_x_vel_ = linear_x_vel; + linear_y_vel_ = linear_y_vel; + angular_vel_ = angular_vel; + + return true; +} + +void Odometry::resetOdometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; +} + +void Odometry::setParams( + const double & robot_radius, const double & wheel_radius, const double & wheel_offset, + const size_t & wheel_count) +{ + robot_radius_ = robot_radius; + wheel_radius_ = wheel_radius; + wheel_offset_ = wheel_offset; + wheels_old_pos_.resize(wheel_count, 0.0); +} + +void Odometry::integrate(const double & dx, const double & dy, const double & dheading) +{ + if (std::fabs(dheading) < 1e-6) + { + // For very small dheading, approximate to linear motion + x_ = x_ + ((dx * std::cos(heading_)) - (dy * std::sin(heading_))); + y_ = y_ + ((dx * std::sin(heading_)) + (dy * std::cos(heading_))); + heading_ = heading_ + dheading; + } + else + { + const double heading_old = heading_; + heading_ = heading_ + dheading; + x_ = x_ + ((dx / dheading) * (std::sin(heading_) - std::sin(heading_old))) + + ((dy / dheading) * (std::cos(heading_) - std::cos(heading_old))); + y_ = y_ - (dx / dheading) * (std::cos(heading_) - std::cos(heading_old)) + + (dy / dheading) * (std::sin(heading_) - std::sin(heading_old)); + } +} + +} // namespace omni_wheel_drive_controller diff --git a/omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp b/omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp new file mode 100644 index 0000000000..0009c4632f --- /dev/null +++ b/omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp @@ -0,0 +1,543 @@ +// Copyright 2025 Aarav Gupta +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#define _USE_MATH_DEFINES + +#include "omni_wheel_drive_controller/omni_wheel_drive_controller.hpp" + +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "tf2/LinearMath/Quaternion.hpp" + +namespace +{ +constexpr auto DEFAULT_COMMAND_IN_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; +} // namespace + +namespace omni_wheel_drive_controller +{ +using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using lifecycle_msgs::msg::State; + +OmniWheelDriveController::OmniWheelDriveController() +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn OmniWheelDriveController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn OmniWheelDriveController::on_configure( + const rclcpp_lifecycle::State &) +{ + auto logger = get_node()->get_logger(); + + params_ = param_listener_->get_params(); + + if (params_.wheel_names.size() < 3) + { + RCLCPP_ERROR( + logger, + "The number of wheels is less than 3. If there are more wheels please pass them all to the " + "controller or use a different one for your wheel configuration."); + return controller_interface::CallbackReturn::ERROR; + } + + odometry_.setParams( + params_.robot_radius, params_.wheel_radius, params_.wheel_offset, params_.wheel_names.size()); + + cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout); + + // Allocate reference interfaces if needed + const int nr_ref_itfs = 3; + reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); + + // Reset the controller + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + + // Initialize velocity command subscriber and define its callback function + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_IN_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) + { + RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->now(); + } + + const auto current_time_diff = get_node()->now() - msg->header.stamp; + + if ( + cmd_vel_timeout_ == rclcpp::Duration::from_seconds(0.0) || + current_time_diff < cmd_vel_timeout_) + { + received_velocity_msg_.set(*msg); + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), + "Ignoring the received message (timestamp %.10f) because it is older than " + "the current time by %.10f seconds, which exceeds the allowed timeout (%.4f)", + rclcpp::Time(msg->header.stamp).seconds(), current_time_diff.seconds(), + cmd_vel_timeout_.seconds()); + } + }); + + // Initialize odometry publisher and message + odometry_publisher_ = get_node()->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_publisher_ = + std::make_shared>( + odometry_publisher_); + + // Append the tf prefix if there is one + std::string tf_prefix = ""; + if (params_.tf_frame_prefix_enable) + { + 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 std::string odom_frame_id = tf_prefix + params_.odom_frame_id; + const std::string base_frame_id = tf_prefix + params_.base_frame_id; + + odometry_message_.header.frame_id = odom_frame_id; + odometry_message_.child_frame_id = base_frame_id; + + // Initialize odom values zeros + odometry_message_.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); + + 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; + odometry_message_.pose.covariance[diagonal_index] = params_.diagonal_covariance.pose[index]; + odometry_message_.twist.covariance[diagonal_index] = params_.diagonal_covariance.twist[index]; + } + + // Initialize transform publisher and message + odometry_transform_publisher_ = get_node()->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_transform_publisher_ = + std::make_shared>( + odometry_transform_publisher_); + + transform_.header.frame_id = odom_frame_id; + transform_.child_frame_id = base_frame_id; + + return controller_interface::CallbackReturn::SUCCESS; +} + +InterfaceConfiguration OmniWheelDriveController::command_interface_configuration() const +{ + std::vector conf_names; + for (const auto & joint_name : params_.wheel_names) + { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + } + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +InterfaceConfiguration OmniWheelDriveController::state_interface_configuration() const +{ + if (params_.open_loop) + { + return {interface_configuration_type::NONE, {}}; + } + + std::vector conf_names; + for (const auto & joint_name : params_.wheel_names) + { + conf_names.push_back(joint_name + "/" + feedback_type()); + } + return {interface_configuration_type::INDIVIDUAL, conf_names}; +} + +controller_interface::CallbackReturn OmniWheelDriveController::on_activate( + const rclcpp_lifecycle::State &) +{ + // Register wheel handles and check if any errors happen + if ( + configure_wheel_handles(params_.wheel_names, registered_wheel_handles_) == + controller_interface::CallbackReturn::ERROR) + { + return controller_interface::CallbackReturn::ERROR; + } + + subscriber_is_active_ = true; + + RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn OmniWheelDriveController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + halt(); + reset_buffers(); + registered_wheel_handles_.clear(); + return controller_interface::CallbackReturn::SUCCESS; +} + +const char * OmniWheelDriveController::feedback_type() const +{ + return params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; +} + +controller_interface::return_type OmniWheelDriveController::update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration &) +{ + auto logger = get_node()->get_logger(); + + auto current_ref_op = received_velocity_msg_.try_get(); + if (current_ref_op.has_value()) + { + command_msg_ = current_ref_op.value(); + } + + const auto age_of_last_command = time - command_msg_.header.stamp; + // Brake if cmd_vel has timeout, override the stored command + if (age_of_last_command > cmd_vel_timeout_) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + reference_interfaces_[2] = 0.0; + } + else if ( + std::isfinite(command_msg_.twist.linear.x) && std::isfinite(command_msg_.twist.linear.y) && + std::isfinite(command_msg_.twist.angular.z)) + { + reference_interfaces_[0] = command_msg_.twist.linear.x; + reference_interfaces_[1] = command_msg_.twist.linear.y; + reference_interfaces_[2] = command_msg_.twist.angular.z; + } + else + { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger, *get_node()->get_clock(), + static_cast(cmd_vel_timeout_.seconds() * 1000), + "Command message contains NaNs. Not updating reference interfaces."); + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type OmniWheelDriveController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration &) +{ + rclcpp::Logger logger = get_node()->get_logger(); + + if ( + !std::isfinite(reference_interfaces_[0]) || !std::isfinite(reference_interfaces_[1]) || + !std::isfinite(reference_interfaces_[2])) + { + // NaNs occur on initialization when the reference interfaces are not yet set + return controller_interface::return_type::OK; + } + + // Update odometry + bool odometry_updated = false; + if (params_.open_loop) + { + odometry_updated = odometry_.updateOpenLoop( + reference_interfaces_[0], reference_interfaces_[1], reference_interfaces_[2], time); + } + else + { + std::vector wheels_feedback(registered_wheel_handles_.size()); // [rads] + for (size_t i = 0; i < static_cast(registered_wheel_handles_.size()); ++i) + { + // Get wheel feedback + const std::optional wheel_feedback_op = + registered_wheel_handles_[i].feedback.value().get().get_optional(); + if (!wheel_feedback_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve data from [%zu] wheel feedback!", i); + return controller_interface::return_type::OK; + } + const double wheel_feedback = wheel_feedback_op.value(); + + if (std::isnan(wheel_feedback)) + { + RCLCPP_ERROR(logger, "The wheel %s is invalid for index [%zu]", feedback_type(), i); + return controller_interface::return_type::ERROR; + } + + wheels_feedback[i] = wheel_feedback; + } + if (params_.position_feedback) + { + odometry_updated = odometry_.updateFromPos(wheels_feedback, time); + } + else + { + odometry_updated = odometry_.updateFromVel(wheels_feedback, time); + } + } + + if (odometry_updated) + { + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + odometry_message_.header.stamp = get_node()->now(); + odometry_message_.pose.pose.position.x = odometry_.getX(); + odometry_message_.pose.pose.position.y = odometry_.getY(); + odometry_message_.pose.pose.orientation.x = orientation.x(); + odometry_message_.pose.pose.orientation.y = orientation.y(); + odometry_message_.pose.pose.orientation.z = orientation.z(); + odometry_message_.pose.pose.orientation.w = orientation.w(); + odometry_message_.twist.twist.linear.x = odometry_.getLinearXVel(); + odometry_message_.twist.twist.linear.y = odometry_.getLinearYVel(); + odometry_message_.twist.twist.angular.z = odometry_.getAngularVel(); + realtime_odometry_publisher_->try_publish(odometry_message_); + + if (params_.enable_odom_tf) + { + transform_.header.stamp = get_node()->now(); + transform_.transform.translation.x = odometry_.getX(); + transform_.transform.translation.y = odometry_.getY(); + transform_.transform.rotation.x = orientation.x(); + transform_.transform.rotation.y = orientation.y(); + transform_.transform.rotation.z = orientation.z(); + transform_.transform.rotation.w = orientation.w(); + tf2_msgs::msg::TFMessage odometry_transform_message; + odometry_transform_message.transforms.push_back(transform_); + realtime_odometry_transform_publisher_->try_publish(odometry_transform_message); + } + } + + compute_and_set_wheel_velocities(); + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn OmniWheelDriveController::on_cleanup( + const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn OmniWheelDriveController::on_error( + const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +void OmniWheelDriveController::compute_and_set_wheel_velocities() +{ + bool set_command_result = true; + + double angle_bw_wheels = (2 * M_PI) / static_cast(registered_wheel_handles_.size()); + for (size_t i = 0; i < static_cast(registered_wheel_handles_.size()); ++i) + { + set_command_result &= registered_wheel_handles_[i].velocity.get().set_value( + ((std::sin((angle_bw_wheels * static_cast(i)) + params_.wheel_offset) * + reference_interfaces_[0]) - + (std::cos((angle_bw_wheels * static_cast(i)) + params_.wheel_offset) * + reference_interfaces_[1]) - + (reference_interfaces_[2] * params_.robot_radius)) / + params_.wheel_radius); + } + + rclcpp::Logger logger = get_node()->get_logger(); + RCLCPP_DEBUG_EXPRESSION( + logger, !set_command_result, "Unable to set the command to one of the command handles!"); +} + +bool OmniWheelDriveController::reset() +{ + odometry_.resetOdometry(); + + reset_buffers(); + + registered_wheel_handles_.clear(); + + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + + return true; +} + +controller_interface::CallbackReturn OmniWheelDriveController::configure_wheel_handles( + const std::vector & wheel_names, std::vector & registered_handles) +{ + rclcpp::Logger logger = get_node()->get_logger(); + + // Register handles + registered_handles.reserve(wheel_names.size()); + for (const auto & wheel_name : wheel_names) + { + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&wheel_name](const auto & interface) + { + return interface.get_prefix_name() == wheel_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.open_loop) + { + registered_handles.emplace_back(WheelHandle{std::nullopt, std::ref(*command_handle)}); + } + else + { + const auto interface_name = feedback_type(); + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&wheel_name, &interface_name](const auto & interface) + { + return interface.get_prefix_name() == wheel_name && + interface.get_interface_name() == interface_name; + }); + + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + registered_handles.emplace_back( + WheelHandle{std::ref(*state_handle), std::ref(*command_handle)}); + } + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +void OmniWheelDriveController::halt() +{ + bool set_command_result = true; + for (const WheelHandle & wheel_handle : registered_wheel_handles_) + { + set_command_result &= wheel_handle.velocity.get().set_value(0.0); + } + rclcpp::Logger logger = get_node()->get_logger(); + RCLCPP_DEBUG_EXPRESSION( + logger, !set_command_result, "Unable to set the command to one of the command handles!"); +} + +void OmniWheelDriveController::reset_buffers() +{ + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); + + // Fill RealtimeBox with NaNs so it will contain a known value + // but still indicate that no command has yet been sent. + command_msg_.header.stamp = get_node()->now(); + command_msg_.twist.linear.x = std::numeric_limits::quiet_NaN(); + command_msg_.twist.linear.y = std::numeric_limits::quiet_NaN(); + command_msg_.twist.linear.z = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.x = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.y = std::numeric_limits::quiet_NaN(); + command_msg_.twist.angular.z = std::numeric_limits::quiet_NaN(); + received_velocity_msg_.set(command_msg_); +} + +bool OmniWheelDriveController::on_set_chained_mode(bool /*chained_mode*/) { return true; } + +std::vector +OmniWheelDriveController::on_export_reference_interfaces() +{ + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + std::vector reference_interface_names = {"/linear/x", "/linear/y", "/angular/z"}; + + 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], hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[i])); + } + + return reference_interfaces; +} +} // namespace omni_wheel_drive_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + omni_wheel_drive_controller::OmniWheelDriveController, + controller_interface::ChainableControllerInterface) diff --git a/omni_wheel_drive_controller/src/omni_wheel_drive_controller_parameters.yaml b/omni_wheel_drive_controller/src/omni_wheel_drive_controller_parameters.yaml new file mode 100644 index 0000000000..3e52baf7b8 --- /dev/null +++ b/omni_wheel_drive_controller/src/omni_wheel_drive_controller_parameters.yaml @@ -0,0 +1,98 @@ +omni_wheel_drive_controller: + wheel_offset: + { + type: double, + description: "Angular offset (radians) of the first wheel from the positive direction of the x-axis of the robot.", + read_only: true, + } + wheel_names: + { + type: string_array, + description: "Names of the wheels' joints, given in an anti-clockwise order starting from the wheel aligned with the positive direction of the x-axis of the robot / offset from it by the value specified in ``wheel_offset``.", + validation: { not_empty<>: [] }, + read_only: true, + } + robot_radius: + { + type: double, + description: "Radius of the robot, distance between the center of the robot and the wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + validation: { gt<>: [0.0] }, + read_only: true, + } + wheel_radius: + { + type: double, + 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 than expected.", + validation: { gt<>: [0.0] }, + read_only: true, + } + tf_frame_prefix_enable: + { + type: bool, + default_value: true, + description: "Enables or disables appending tf_prefix to tf frame id's.", + read_only: true, + } + 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.", + read_only: true, + } + odom_frame_id: + { + type: string, + default_value: "odom", + description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", + read_only: true, + } + base_frame_id: + { + type: string, + default_value: "base_link", + description: "Name of the robot's base frame that is child of the odometry frame.", + read_only: true, + } + diagonal_covariance: + pose: + { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + read_only: true, + } + twist: + { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + read_only: true, + } + open_loop: + { + type: bool, + default_value: false, + description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", + read_only: true, + } + position_feedback: + { + type: bool, + default_value: true, + description: "Only valid if ``open_loop`` is set to false. If there is position feedback from the hardware, set the parameter to ``true``, else set it to ``false``.", + read_only: true, + } + enable_odom_tf: + { + type: bool, + default_value: true, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + read_only: true, + } + cmd_vel_timeout: { + type: double, + default_value: 0.5, # seconds + description: "Timeout in seconds, after which input command on ``~/cmd_vel`` topic is considered stale.", + read_only: true, + } diff --git a/omni_wheel_drive_controller/test/config/test_omni_wheel_drive_controller.yaml b/omni_wheel_drive_controller/test/config/test_omni_wheel_drive_controller.yaml new file mode 100644 index 0000000000..bd811cbae0 --- /dev/null +++ b/omni_wheel_drive_controller/test/config/test_omni_wheel_drive_controller.yaml @@ -0,0 +1,25 @@ +test_omni_wheel_drive_controller: + ros__parameters: + wheel_offset: 0.0 + wheel_names: + [ + "front_wheel_joint", + "left_wheel_joint", + "back_wheel_joint", + "right_wheel_joint", + ] + + robot_radius: 0.20 + wheel_radius: 0.02 + + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + open_loop: true + position_feedback: false + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + publish_limited_velocity: true diff --git a/omni_wheel_drive_controller/test/test_load_omni_wheel_drive_controller.cpp b/omni_wheel_drive_controller/test/test_load_omni_wheel_drive_controller.cpp new file mode 100644 index 0000000000..9cdd2bb0b0 --- /dev/null +++ b/omni_wheel_drive_controller/test/test_load_omni_wheel_drive_controller.cpp @@ -0,0 +1,50 @@ +// Copyright 2025 Aarav Gupta +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "controller_manager/controller_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(TestLoadOmniWheelDriveController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/config/test_omni_wheel_drive_controller.yaml"; + + cm.set_parameter({"test_omni_wheel_drive_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_omni_wheel_drive_controller.type", + "omni_wheel_drive_controller/OmniWheelDriveController"}); + + ASSERT_NE(cm.load_controller("test_omni_wheel_drive_controller"), 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/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp new file mode 100644 index 0000000000..d0ecb366a2 --- /dev/null +++ b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.cpp @@ -0,0 +1,878 @@ +// Copyright 2025 Aarav Gupta +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_omni_wheel_drive_controller.hpp" +#include +#include "controller_interface/controller_interface_base.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +using lifecycle_msgs::msg::State; + +class OmniWheelDriveControllerTest +: public OmniWheelDriveControllerFixture +{ +}; + +TEST_F(OmniWheelDriveControllerTest, init_fails_without_parameters) +{ + const auto ret = + controller_->init(controller_name_, urdf_, 0, "", controller_->define_custom_node_options()); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); +} + +TEST_F(OmniWheelDriveControllerTest, configure_fails_with_less_than_three_wheels) +{ + ASSERT_EQ( + InitController({"first_wheel_joint", "second_wheel_joint"}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +TEST_F(OmniWheelDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Check command interfaces configuration + auto command_interfaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_interfaces.names.size(), wheel_names_.size()); + for (size_t i = 0; i < command_interfaces.names.size(); ++i) + { + EXPECT_EQ(command_interfaces.names[i], wheel_names_[i] + "/" + HW_IF_VELOCITY); + } + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // Check state interfaces configuration + auto state_interfaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_interfaces.names.size(), wheel_names_.size()); + for (size_t i = 0; i < state_interfaces.names.size(); ++i) + { + EXPECT_EQ(state_interfaces.names[i], wheel_names_[i] + "/" + HW_IF_POSITION); + } + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // Check reference interfaces configuration + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), 3); + + for (size_t i = 0; i < reference_interface_names.size(); ++i) + { + const std::string ref_itf_prefix_name = + std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names[i]; + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); + } +} + +TEST_F(OmniWheelDriveControllerTest, 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"; + + ASSERT_EQ( + InitController( + wheel_names_, 0.0, + {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()), NODE_SUCCESS); + + nav_msgs::msg::Odometry odometry_message = controller_->odometry_message_; + 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(OmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + ASSERT_EQ( + InitController( + wheel_names_, 0.0, + {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()), NODE_SUCCESS); + + nav_msgs::msg::Odometry odometry_message = controller_->odometry_message_; + 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(OmniWheelDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; + + ASSERT_EQ( + InitController( + wheel_names_, 0.0, + {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()), NODE_SUCCESS); + + nav_msgs::msg::Odometry odometry_message = controller_->odometry_message_; + 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(OmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_false_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( + wheel_names_, 0.0, + {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()), NODE_SUCCESS); + + nav_msgs::msg::Odometry odometry_message = controller_->odometry_message_; + 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(OmniWheelDriveControllerTest, 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( + wheel_names_, 0.0, + {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()), NODE_SUCCESS); + + nav_msgs::msg::Odometry odometry_message = controller_->odometry_message_; + 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(OmniWheelDriveControllerTest, 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( + wheel_names_, 0.0, + {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()), NODE_SUCCESS); + + nav_msgs::msg::Odometry odometry_message = controller_->odometry_message_; + 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); +} + +TEST_F(OmniWheelDriveControllerTest, activate_fails_without_resources_assigned) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_ERROR); +} + +TEST_F(OmniWheelDriveControllerTest, activate_succeeds_with_pos_resources_assigned) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + // We implicitly test that by default position feedback is required + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + assignResourcesPosFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(OmniWheelDriveControllerTest, activate_succeeds_with_vel_resources_assigned) +{ + ASSERT_EQ( + InitController( + wheel_names_, 0.0, {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + assignResourcesVelFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(OmniWheelDriveControllerTest, activate_fails_with_wrong_resources_assigned_1) +{ + ASSERT_EQ( + InitController( + wheel_names_, 0.0, {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + assignResourcesPosFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_ERROR); +} + +TEST_F(OmniWheelDriveControllerTest, activate_fails_with_wrong_resources_assigned_2) +{ + ASSERT_EQ( + InitController( + wheel_names_, 0.0, {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + assignResourcesVelFeedback(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_ERROR); +} + +// When not in chained mode, we want to test that +// 1. The controller is configurable and all lifecycle functions work properly +// 2. command_interfaces are set to 0.0 when cmd_vel_timeout_ is exceeded and on deactivation +// 3. command_interfaces are set to correct command values the command messages are not timed-out. +// In particular, make sure that the command_interface is not set to NaN right when it starts up. +TEST_F(OmniWheelDriveControllerTest, chainable_controller_unchained_mode) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->is_chainable()); + ASSERT_TRUE(controller_->set_chained_mode(false)); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(executor); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + for (size_t i = 0; i < command_itfs_.size(); i++) + { + ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + } + + // Check that a late command message causes the command interfaces to be set to 0.0 + publish_twist(); + + // delay enough time to trigger the timeout (cmd_vel_timeout_ = 0.5s) + controller_->wait_for_twist(executor); + std::this_thread::sleep_for(std::chrono::milliseconds(501)); + + ASSERT_EQ( + controller_->update( + cmd_vel_publisher_node_->get_clock()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // Now check that a timely published command message sets the command interfaces to the correct + // values + publish_twist(); + // wait for msg to be published to the system + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + } + + // Now check that the command interfaces are set to 0.0 on deactivation + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + +// When in chained mode, we want to test that +// 1. The controller is configurable and all lifecycle functions work properly +// 2. command_interfaces are set to 0.0 on deactivation +// 3. command_interfaces are set to correct command values (not set to NaN right when it starts up) +TEST_F(OmniWheelDriveControllerTest, chainable_controller_chained_mode) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->is_chainable()); + ASSERT_TRUE(controller_->set_chained_mode(true)); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(executor); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + for (size_t i = 0; i < command_itfs_.size(); i++) + { + ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + } + + // Imitate preceding controllers by setting reference_interfaces_ + controller_->reference_interfaces_[0] = 1.0; + controller_->reference_interfaces_[1] = 1.0; + controller_->reference_interfaces_[2] = 1.0; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + } + + // Now check that the command interfaces are set to 0.0 on deactivation + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + +// Make sure that the controller is properly reset when deactivated +// and accepts new commands as expected when it is activated again. +TEST_F(OmniWheelDriveControllerTest, deactivate_then_activate) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + // choose radius = 1 so that the command values (rev/s) are the same as the linear velocity (m/s) + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->set_chained_mode(false)); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(executor); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + for (size_t i = 0; i < command_itfs_.size(); i++) + { + ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + } + + // Now check that a timely published command message sets the command interfaces to the correct + // values + publish_twist(); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + } + + // Now check that the command interfaces are set to 0.0 on deactivation + // (despite calls to update()) + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // Activate again + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(executor); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)) + << "Reference interfaces should initially be NaN on activation"; + } + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // A new command should work as expected + publish_twist(); + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + } + + // Deactivate again and cleanup + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + state = controller_->get_node()->cleanup(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + executor.cancel(); +} + +TEST_F(OmniWheelDriveControllerTest, command_with_zero_timestamp_is_accepted_with_warning) +{ + ASSERT_EQ(InitController(), controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_TRUE(controller_->set_chained_mode(false)); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResourcesPosFeedback(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(executor); + + // published command message with zero timestamp sets the command interfaces to the correct values + publish_twist_timestamped(rclcpp::Time(0, 0, RCL_ROS_TIME)); + // wait for msg is be published to the system + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + std::vector expected_wheels_vel_cmds = {-15.0, 5.0, 5.0, -15.0}; + for (size_t i = 0; i < command_itfs_.size(); i++) + { + EXPECT_DOUBLE_EQ(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i]); + } + + // Deactivate and cleanup + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + state = controller_->get_node()->cleanup(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + executor.cancel(); +} + +TEST_F(OmniWheelDriveControllerTest, 3_wheel_test) +{ + ASSERT_EQ( + InitController({"wheel_1", "wheel_2", "wheel_3"}, 0.0), controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + wheels_pos_states_ = {1, 1, 1}; + wheels_vel_states_ = {1, 1, 1}; + wheels_vel_cmds_ = {0.1, 0.2, 0.3}; + assignResourcesPosFeedback({"wheel_1", "wheel_2", "wheel_3"}); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(executor); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + for (size_t i = 0; i < 3; i++) + { + ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + } + + // Check that a published command msg sets the command interfaces to the correct values + publish_twist(); + // wait for msg to be published to the system + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + std::vector expected_wheels_vel_cmds = {-15.0, 8.66025, -8.66025}; + for (size_t i = 0; i < 3; i++) + { + EXPECT_NEAR(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); + } + + // Now check that the command interfaces are set to 0.0 on deactivation + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + for (size_t i = 0; i < 3; i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + for (size_t i = 0; i < 3; i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + +TEST_F(OmniWheelDriveControllerTest, 3_wheel_rot_test) +{ + ASSERT_EQ( + InitController({"wheel_1", "wheel_2", "wheel_3"}, 1.0471975512), + controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + wheels_pos_states_ = {1, 1, 1}; + wheels_vel_states_ = {1, 1, 1}; + wheels_vel_cmds_ = {0.1, 0.2, 0.3}; + assignResourcesPosFeedback({"wheel_1", "wheel_2", "wheel_3"}); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(executor); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + for (size_t i = 0; i < 3; i++) + { + ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + } + + // Check that a published command msg sets the command interfaces to the correct values + publish_twist(); + // wait for msg to be published to the system + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + std::vector expected_wheels_vel_cmds = {-1.33975, 5.0, -18.6603}; + for (size_t i = 0; i < 3; i++) + { + EXPECT_NEAR(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); + } + + // Now check that the command interfaces are set to 0.0 on deactivation + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + for (size_t i = 0; i < 3; i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + for (size_t i = 0; i < 3; i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + +TEST_F(OmniWheelDriveControllerTest, 4_wheel_rot_test) +{ + ASSERT_EQ( + InitController({"wheel_1", "wheel_2", "wheel_3", "wheel_4"}, 0.7853981634), + controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + wheels_pos_states_ = {1, 1, 1, 1}; + wheels_vel_states_ = {1, 1, 1, 1}; + wheels_vel_cmds_ = {0.1, 0.2, 0.3, 0.4}; + assignResourcesPosFeedback({"wheel_1", "wheel_2", "wheel_3", "wheel_4"}); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(executor); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + for (size_t i = 0; i < 4; i++) + { + ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + } + + // Check that a published command msg sets the command interfaces to the correct values + publish_twist(); + // wait for msg to be published to the system + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + std::vector expected_wheels_vel_cmds = {-5.0, 9.14214, -5.0, -19.1421}; + for (size_t i = 0; i < 4; i++) + { + EXPECT_NEAR(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); + } + + // Now check that the command interfaces are set to 0.0 on deactivation + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + for (size_t i = 0; i < 4; i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + for (size_t i = 0; i < 4; i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + +TEST_F(OmniWheelDriveControllerTest, 5_wheel_test) +{ + ASSERT_EQ( + InitController({"wheel_1", "wheel_2", "wheel_3", "wheel_4", "wheel_5"}, 0.0), + controller_interface::return_type::OK); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + wheels_pos_states_ = {1, 1, 1, 1, 1}; + wheels_vel_states_ = {1, 1, 1, 1, 1}; + wheels_vel_cmds_ = {0.1, 0.2, 0.3, 0.4, 0.5}; + assignResourcesPosFeedback({"wheel_1", "wheel_2", "wheel_3", "wheel_4", "wheel_5"}); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(executor); + + // Reference interfaces should be NaN on initialization + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + // But NaNs should not propagate to command interfaces + for (size_t i = 0; i < 5; i++) + { + ASSERT_FALSE(std::isnan(command_itfs_[i].get_optional().value())); + } + + // Check that a published command msg sets the command interfaces to the correct values + publish_twist(); + // wait for msg to be published to the system + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + std::vector expected_wheels_vel_cmds = {-15.0, 1.42040, 8.96802, -2.78768, -17.6007}; + for (size_t i = 0; i < 5; i++) + { + EXPECT_NEAR(command_itfs_[i].get_optional().value(), expected_wheels_vel_cmds[i], 0.0001); + } + + // Now check that the command interfaces are set to 0.0 on deactivation + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + for (size_t i = 0; i < 5; i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + for (size_t i = 0; i < 5; i++) + { + EXPECT_EQ(command_itfs_[i].get_optional().value(), 0.0); + } + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} + +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/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp new file mode 100644 index 0000000000..6e0bc45a36 --- /dev/null +++ b/omni_wheel_drive_controller/test/test_omni_wheel_drive_controller.hpp @@ -0,0 +1,248 @@ +// Copyright 2025 Aarav Gupta +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ +#define TEST_OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "hardware_interface/handle.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "omni_wheel_drive_controller/omni_wheel_drive_controller.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" + +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +std::vector wheel_names_ = { + "front_wheel_joint", "left_wheel_joint", "back_wheel_joint", "right_wheel_joint"}; +} // namespace + +// subclassing and friending so we can access member variables +class TestableOmniWheelDriveController +: public omni_wheel_drive_controller::OmniWheelDriveController +{ + FRIEND_TEST(OmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_namespace); + FRIEND_TEST(OmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace); + FRIEND_TEST(OmniWheelDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_namespace); + FRIEND_TEST(OmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_false_set_namespace); + FRIEND_TEST(OmniWheelDriveControllerTest, configure_succeeds_tf_test_prefix_true_set_namespace); + FRIEND_TEST(OmniWheelDriveControllerTest, configure_succeeds_tf_blank_prefix_true_set_namespace); + FRIEND_TEST(OmniWheelDriveControllerTest, cleanup); + FRIEND_TEST(OmniWheelDriveControllerTest, chainable_controller_unchained_mode); + FRIEND_TEST(OmniWheelDriveControllerTest, chainable_controller_chained_mode); + FRIEND_TEST(OmniWheelDriveControllerTest, deactivate_then_activate); + FRIEND_TEST(OmniWheelDriveControllerTest, command_with_zero_timestamp_is_accepted_with_warning); + FRIEND_TEST(OmniWheelDriveControllerTest, 3_wheel_test); + FRIEND_TEST(OmniWheelDriveControllerTest, 3_wheel_rot_test); + FRIEND_TEST(OmniWheelDriveControllerTest, 4_wheel_rot_test); + FRIEND_TEST(OmniWheelDriveControllerTest, 5_wheel_test); + + /** + * @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)) + { + 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)); + } + } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class OmniWheelDriveControllerFixture : public ::testing::Test +{ +public: + void SetUp() + { + // Initialize controller + controller_ = std::make_unique(); + + cmd_vel_publisher_node_ = std::make_shared("cmd_vel_publisher"); + cmd_vel_publisher_ = + cmd_vel_publisher_node_->create_publisher( + "/test_omni_wheel_drive_controller/cmd_vel", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + +protected: + void publish_twist(double linear_x = 1.0, double linear_y = 1.0, double angular = 1.0) + { + publish_twist_timestamped( + cmd_vel_publisher_node_->get_clock()->now(), linear_x, linear_y, angular); + } + + void publish_twist_timestamped( + const rclcpp::Time & stamp, const double & twist_linear_x = 1.0, + const double & twist_linear_y = 1.0, const double & twist_angular_z = 1.0) + { + const char * topic_name = cmd_vel_publisher_->get_topic_name(); + size_t wait_count = 0; + while (cmd_vel_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; + } + + geometry_msgs::msg::TwistStamped 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; + + cmd_vel_publisher_->publish(msg); + } + + /// \brief wait for the subscriber and publisher to completely setup + void waitForSetup(rclcpp::Executor & executor) + { + constexpr std::chrono::seconds TIMEOUT{2}; + auto clock = cmd_vel_publisher_node_->get_clock(); + auto start = clock->now(); + while (cmd_vel_publisher_->get_subscription_count() <= 0) + { + if ((clock->now() - start) > TIMEOUT) + { + FAIL(); + } + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void assignResourcesPosFeedback(const std::vector wheel_names = wheel_names_) + { + std::vector state_ifs; + state_ifs.reserve(wheels_pos_states_.size()); + state_itfs_.reserve(wheels_pos_states_.size()); + for (size_t i = 0; i < wheels_pos_states_.size(); ++i) + { + state_itfs_.emplace_back( + hardware_interface::StateInterface(wheel_names[i], HW_IF_POSITION, &wheels_pos_states_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + std::vector command_ifs; + command_ifs.reserve(wheels_vel_cmds_.size()); + command_itfs_.reserve(wheels_vel_cmds_.size()); + for (size_t i = 0; i < wheels_vel_cmds_.size(); ++i) + { + command_itfs_.emplace_back( + hardware_interface::CommandInterface(wheel_names[i], HW_IF_VELOCITY, &wheels_vel_cmds_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void assignResourcesVelFeedback() + { + std::vector state_ifs; + state_ifs.reserve(wheels_vel_states_.size()); + state_itfs_.reserve(wheels_vel_states_.size()); + for (size_t i = 0; i < wheels_vel_states_.size(); ++i) + { + state_itfs_.emplace_back( + hardware_interface::StateInterface( + wheel_names_[i], HW_IF_VELOCITY, &wheels_vel_states_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + std::vector command_ifs; + command_ifs.reserve(wheels_vel_cmds_.size()); + command_itfs_.reserve(wheels_vel_cmds_.size()); + for (size_t i = 0; i < wheels_vel_cmds_.size(); ++i) + { + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + wheel_names_[i], HW_IF_VELOCITY, &wheels_vel_cmds_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + controller_interface::return_type InitController( + const std::vector wheel_joints_init = wheel_names_, + const double wheel_offset = 0.0, const std::vector & parameters = {}, + const std::string ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("wheel_names", rclcpp::ParameterValue(wheel_joints_init))); + parameter_overrides.push_back( + rclcpp::Parameter("wheel_offset", rclcpp::ParameterValue(wheel_offset))); + parameter_overrides.push_back(rclcpp::Parameter("robot_radius", rclcpp::ParameterValue(0.5))); + 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_, urdf_, 0, ns, node_options); + } + + std::vector wheels_pos_states_ = {1, 1, 1, 1}; + std::vector wheels_vel_states_ = {1, 1, 1, 1}; + std::vector wheels_vel_cmds_ = {0.1, 0.2, 0.3, 0.4}; + + std::vector state_itfs_; + std::vector command_itfs_; + + std::vector reference_interface_names = {"linear/x", "linear/y", "angular/z"}; + + std::unique_ptr controller_; + rclcpp::Node::SharedPtr cmd_vel_publisher_node_; + rclcpp::Publisher::SharedPtr cmd_vel_publisher_; + + const std::string urdf_ = ""; + std::string controller_name_ = "test_omni_wheel_drive_controller"; +}; + +#endif // TEST_OMNI_WHEEL_DRIVE_CONTROLLER_HPP_ diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 60258ceaf9..05b8e37db9 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,49 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Fix: Remove deprecated `rclcpp::spin_some(node)` (backport `#1928 `_) (`#1932 `_) +* Don't call `release_interfaces` from controllers (backport `#1910 `_) (`#1911 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index 2235863f68..716d2b11f5 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -36,9 +36,16 @@ target_include_directories(parallel_gripper_action_controller PUBLIC $ ) target_link_libraries(parallel_gripper_action_controller PUBLIC - parallel_gripper_action_controller_parameters -) -ament_target_dependencies(parallel_gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + parallel_gripper_action_controller_parameters + control_toolbox::control_toolbox + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_action::rclcpp_action + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${control_msgs_TARGETS}) pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) @@ -50,9 +57,9 @@ if(BUILD_TESTING) add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") ament_add_gmock(test_load_parallel_gripper_action_controllers test/test_load_parallel_gripper_action_controller.cpp) target_include_directories(test_load_parallel_gripper_action_controllers PRIVATE include) - ament_target_dependencies(test_load_parallel_gripper_action_controllers - controller_manager - ros2_control_test_assets + target_link_libraries(test_load_parallel_gripper_action_controllers + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) add_rostest_with_parameters_gmock(test_parallel_gripper_controller diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp index 0bd48b65f5..93a6f6ec65 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -36,8 +36,8 @@ #include "controller_interface/controller_interface.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_server_goal_handle.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" // Project #include "parallel_gripper_controller/parallel_gripper_action_controller_parameters.hpp" @@ -93,10 +93,6 @@ class GripperActionController : public controller_interface::ControllerInterface controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - realtime_tools::RealtimeBuffer command_; - // pre-allocated memory that is reused to set the realtime buffer - Commands command_struct_, command_struct_rt_; - protected: using GripperCommandAction = control_msgs::action::ParallelGripperCommand; using ActionServer = rclcpp_action::Server; @@ -105,12 +101,16 @@ class GripperActionController : public controller_interface::ControllerInterface using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; - using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + using RealtimeGoalHandleBox = realtime_tools::RealtimeThreadSafeBox; - bool update_hold_position_; + // the realtime container to exchange the reference from subscriber + realtime_tools::RealtimeThreadSafeBox command_; + // pre-allocated memory that is reused + Commands command_struct_; + // save the last reference in case of unable to get value from box + Commands command_struct_rt_; - bool verbose_ = false; ///< Hard coded verbose flag to help in debugging - std::string name_; ///< Controller name. + std::string name_; ///< Controller name. std::optional> joint_command_interface_; std::optional> @@ -125,7 +125,7 @@ class GripperActionController : public controller_interface::ControllerInterface std::shared_ptr param_listener_; Params params_; - RealtimeGoalHandleBuffer + RealtimeGoalHandleBox rt_active_goal_; ///< Container for the currently active action goal, if any. control_msgs::action::ParallelGripperCommand::Result::SharedPtr pre_alloc_result_; diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 50f91678cb..479c255a25 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -30,12 +30,14 @@ namespace parallel_gripper_action_controller void GripperActionController::preempt_active_goal() { // Cancels the currently active goal - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const RealtimeGoalHandlePtr & goal) { active_goal = goal; }); if (active_goal) { // Marks the current goal as canceled active_goal->setCanceled(std::make_shared()); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } } @@ -57,22 +59,50 @@ controller_interface::CallbackReturn GripperActionController::on_init() controller_interface::return_type GripperActionController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - command_struct_rt_ = *(command_.readFromRT()); + auto logger = get_node()->get_logger(); + auto command_struct_rt_op = command_.try_get(); + if (command_struct_rt_op.has_value()) + { + command_struct_rt_ = command_struct_rt_op.value(); + } + const auto current_position_op = joint_position_state_interface_->get().get_optional(); + if (!current_position_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve current position value"); + return controller_interface::return_type::OK; + } + const auto current_velocity_op = joint_velocity_state_interface_->get().get_optional(); + if (!current_velocity_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve current velocity value"); + return controller_interface::return_type::OK; + } - const double current_position = joint_position_state_interface_->get().get_value(); - const double current_velocity = joint_velocity_state_interface_->get().get_value(); - const double error_position = command_struct_rt_.position_cmd_ - current_position; + const double error_position = command_struct_rt_.position_cmd_ - current_position_op.value(); - check_for_success(get_node()->now(), error_position, current_position, current_velocity); + check_for_success( + get_node()->now(), error_position, current_position_op.value(), current_velocity_op.value()); - joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_); - if (speed_interface_.has_value()) + if (!joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_)) + { + RCLCPP_WARN( + logger, "Unable to set the joint position command to: %f", command_struct_rt_.position_cmd_); + return controller_interface::return_type::OK; + } + if ( + speed_interface_.has_value() && + !speed_interface_->get().set_value(command_struct_rt_.max_velocity_)) { - speed_interface_->get().set_value(command_struct_rt_.max_velocity_); + RCLCPP_WARN(logger, "Unable to set the speed command to: %f", command_struct_rt_.max_velocity_); + + return controller_interface::return_type::OK; } - if (effort_interface_.has_value()) + if ( + effort_interface_.has_value() && + !effort_interface_->get().set_value(command_struct_rt_.max_effort_)) { - effort_interface_->get().set_value(command_struct_rt_.max_effort_); + RCLCPP_WARN(logger, "Unable to set the effort command to: %f", command_struct_rt_.max_effort_); + return controller_interface::return_type::OK; } return controller_interface::return_type::OK; @@ -124,14 +154,14 @@ void GripperActionController::accepted_callback( { command_struct_.max_effort_ = params_.max_effort; } - command_.writeFromNonRT(command_struct_); + command_.set(command_struct_); 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); + rt_active_goal_.set([rt_goal](RealtimeGoalHandlePtr & stored_value) { stored_value = rt_goal; }); // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); @@ -148,7 +178,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback( RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const RealtimeGoalHandlePtr & goal) { active_goal = goal; }); if (active_goal && active_goal->gh_ == goal_handle) { // Enter hold current position mode @@ -161,24 +192,31 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback( auto action_res = std::make_shared(); active_goal->setCanceled(action_res); // Reset current goal - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } return rclcpp_action::CancelResponse::ACCEPT; } void GripperActionController::set_hold_position() { - command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + const auto position_op = joint_position_state_interface_->get().get_optional(); + if (!position_op.has_value()) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve data of joint position"); + } + command_struct_.position_cmd_ = position_op.value(); command_struct_.max_effort_ = params_.max_effort; command_struct_.max_velocity_ = params_.max_velocity; - command_.writeFromNonRT(command_struct_); + command_.set(command_struct_); } void GripperActionController::check_for_success( const rclcpp::Time & time, double error_position, double current_position, double current_velocity) { - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&](const RealtimeGoalHandlePtr & goal) { active_goal = goal; }); if (!active_goal) { return; @@ -192,7 +230,8 @@ void GripperActionController::check_for_success( pre_alloc_result_->stalled = false; RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); active_goal->setSucceeded(pre_alloc_result_); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } else { @@ -217,7 +256,8 @@ void GripperActionController::check_for_success( RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); active_goal->setAborted(pre_alloc_result_); } - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set([](RealtimeGoalHandlePtr & stored_value) + { stored_value = RealtimeGoalHandlePtr(); }); } } } @@ -317,10 +357,18 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } // Command - non RT version - command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + const auto position_op = joint_position_state_interface_->get().get_optional(); + if (!position_op.has_value()) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve data of joint position"); + } + else + { + command_struct_.position_cmd_ = position_op.value(); + } command_struct_.max_effort_ = params_.max_effort; command_struct_.max_velocity_ = params_.max_velocity; - command_.initRT(command_struct_); + command_.try_set(command_struct_); // Result pre_alloc_result_ = std::make_shared(); @@ -347,7 +395,6 @@ controller_interface::CallbackReturn GripperActionController::on_deactivate( joint_command_interface_ = std::nullopt; joint_position_state_interface_ = std::nullopt; joint_velocity_state_interface_ = std::nullopt; - release_interfaces(); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 7c6e05bbd6..82a2012b1f 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.24.0 + 4.33.1 The parallel_gripper_controller package Bence Magyar diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp index 23c13b157a..1831e8ab92 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -80,9 +80,11 @@ TEST_F(GripperControllerTest, ConfigureParamsSuccess) { this->SetUpController(); - this->controller_->get_node()->set_parameter({"joint", "joint1"}); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); - rclcpp::spin_some(this->controller_->get_node()->get_node_base_interface()); + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + executor.spin_some(); // configure successful ASSERT_EQ( @@ -132,6 +134,7 @@ TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) ASSERT_EQ( this->controller_->on_deactivate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + this->controller_->release_interfaces(); // re-assign interfaces std::vector command_ifs; diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 6467cbae80..f734073420 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,59 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- +* Fix upstream Pid class deprecation warnings (`#1959 `_) +* Contributors: Christoph Fröhlich + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- +* Apply API change of PidROS (backport `#1823 `_) (`#1826 `_) +* Change the tests to work without deprecated PID settings (backport `#1824 `_) (`#1825 `_) +* Contributors: mergify[bot] + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- +* Add new members for PID controller parameters (backport `#1585 `_) (`#1769 `_) +* Set enable_feedforward parameter in the respective tests (backport `#1743 `_) (`#1744 `_) +* Contributors: Victor Coutinho Vieira Santos, Sai Kishor Kothakota, Christoph Fröhlich + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- +* [Pid] Add enable_feedforward parameter (backport `#1553 `_) (`#1689 `_) +* Simplify `on_set_chained_mode` avoiding cpplint warnings (backport `#1564 `_) (`#1688 `_) +* Contributors: mergify[bot], Pascal Auf der Maur, hagyesh Agresar + 4.24.0 (2025-04-27) ------------------- diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 20397570b0..afe6ebf5f0 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -49,9 +49,19 @@ 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}) + pid_controller_parameters + angles::angles + control_toolbox::control_toolbox + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${std_srvs_TARGETS} + ${control_msgs_TARGETS} + ${parameter_traits_TARGETS} + ${std_srvs_TARGETS}) pluginlib_export_plugin_description_file(controller_interface pid_controller.xml) @@ -67,11 +77,6 @@ if(BUILD_TESTING) ) 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 @@ -80,11 +85,6 @@ if(BUILD_TESTING) ) 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 @@ -93,18 +93,12 @@ if(BUILD_TESTING) ) 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 + target_link_libraries(test_load_pid_controller + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) endif() diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index fc94357ab3..03a760040d 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -74,9 +74,15 @@ Services - /set_feedforward_control [std_srvs/srv/SetBool] +.. warning:: + This service is being deprecated in favour of setting the ``feedforward_gain`` parameter to a non-zero value. + Publishers ,,,,,,,,,,, - /controller_state [control_msgs/msg/MultiDOFStateStamped] +- //pid_state [control_msgs/msg/PidState] + +Initially the PidState publisher is turned off. It can be turned on by using ``gains..activate_state_publisher`` parameter. Parameters ,,,,,,,,,,, @@ -85,6 +91,10 @@ The PID controller uses the `generate_parameter_library #include #include #include @@ -27,8 +28,8 @@ #include "control_toolbox/pid_ros.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "std_srvs/srv/set_bool.hpp" #include "pid_controller/pid_controller_parameters.hpp" @@ -36,12 +37,6 @@ namespace pid_controller { -enum class feedforward_mode_type : std::uint8_t -{ - OFF = 0, - ON = 1, -}; - class PidController : public controller_interface::ChainableControllerInterface { public: @@ -88,18 +83,21 @@ class PidController : public controller_interface::ChainableControllerInterface // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; + realtime_tools::RealtimeThreadSafeBox input_ref_; + ControllerReferenceMsg current_ref_; rclcpp::Subscription::SharedPtr measured_state_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> measured_state_; + realtime_tools::RealtimeThreadSafeBox measured_state_; + ControllerMeasuredStateMsg current_state_; rclcpp::Service::SharedPtr set_feedforward_control_service_; - realtime_tools::RealtimeBuffer control_mode_; + realtime_tools::RealtimeBuffer feedforward_mode_enabled_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr s_publisher_; std::unique_ptr state_publisher_; + ControllerStateMsg state_msg_; // override methods from ChainableControllerInterface std::vector on_export_reference_interfaces() override; diff --git a/pid_controller/package.xml b/pid_controller/package.xml index f4acb87f2b..07f7ca31f7 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.24.0 + 4.33.1 Controller based on PID implememenation from control_toolbox package. Bence Magyar diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index e0ef70ffac..38601c26d6 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -53,15 +53,15 @@ using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceM // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, const std::vector & dof_names) + ControllerCommandMsg & 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()); + 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) + ControllerCommandMsg & msg, const std::vector & dof_names) { reset_controller_reference_msg(msg, dof_names); } @@ -74,7 +74,7 @@ PidController::PidController() : controller_interface::ChainableControllerInterf controller_interface::CallbackReturn PidController::on_init() { - control_mode_.initRT(feedforward_mode_type::OFF); + feedforward_mode_enabled_.initRT(false); try { @@ -96,6 +96,8 @@ void PidController::update_parameters() return; } params_ = param_listener_->get_params(); + + feedforward_mode_enabled_.writeFromNonRT(params_.enable_feedforward); } controller_interface::CallbackReturn PidController::configure_parameters() @@ -137,9 +139,8 @@ controller_interface::CallbackReturn PidController::configure_parameters() 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); + pids_[i] = std::make_shared( + get_node(), "gains." + params_.dof_names[i], "~/" + params_.dof_names[i], false); if (!pids_[i]->initialize_from_ros_parameters()) { return CallbackReturn::FAILURE; @@ -177,9 +178,8 @@ controller_interface::CallbackReturn PidController::on_configure( "~/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); + reset_controller_reference_msg(current_ref_, reference_and_state_dof_names_); + input_ref_.set(current_ref_); // input state Subscriber and callback if (params_.use_external_measured_states) @@ -225,15 +225,15 @@ controller_interface::CallbackReturn PidController::on_configure( } } // TODO(destogl): Sort the input values based on joint and interface names - measured_state_.writeFromNonRT(state_msg); + measured_state_.set(*state_msg); }; measured_state_subscriber_ = get_node()->create_subscription( "~/measured_state", subscribers_qos, measured_state_callback); } - std::shared_ptr measured_state_msg = - std::make_shared(); + + ControllerMeasuredStateMsg measured_state_msg; reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); - measured_state_.writeFromNonRT(measured_state_msg); + measured_state_.set(measured_state_msg); measured_state_values_.resize( dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); @@ -243,14 +243,13 @@ controller_interface::CallbackReturn PidController::on_configure( 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); - } + feedforward_mode_enabled_.writeFromNonRT(request->data); + + RCLCPP_WARN( + get_node()->get_logger(), + "This service will be deprecated in favour of setting the ``feedforward_gain`` parameter to " + "a non-zero value."); + response->success = true; }; @@ -273,13 +272,11 @@ controller_interface::CallbackReturn PidController::on_configure( } // Reserve memory in state publisher - state_publisher_->lock(); - state_publisher_->msg_.dof_states.resize(reference_and_state_dof_names_.size()); + state_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_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; @@ -295,7 +292,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names = reference_and_state_dof_names_; - input_ref_.writeFromNonRT(ref_msg); + input_ref_.set(*ref_msg); } else if ( msg->dof_names.size() == reference_and_state_dof_names_.size() && @@ -304,7 +301,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names.size(); ++i) @@ -327,7 +324,7 @@ void PidController::reference_callback(const std::shared_ptr PidController::on_export_state_i return state_interfaces; } -bool PidController::on_set_chained_mode(bool chained_mode) -{ - // Always accept switch to/from chained mode - return true || chained_mode; -} +bool PidController::on_set_chained_mode(bool /*chained_mode*/) { return true; } 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_); + auto input_ref_op = input_ref_.try_get(); + if (input_ref_op.has_value()) + { + current_ref_ = input_ref_op.value(); + reset_controller_reference_msg(current_ref_, reference_and_state_dof_names_); + input_ref_.try_set(current_ref_); + } + auto measured_state_op = measured_state_.try_get(); + if (measured_state_op.has_value()) + { + reset_controller_measured_state_msg(current_state_, reference_and_state_dof_names_); + measured_state_.try_set(current_state_); + } reference_interfaces_.assign( reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); @@ -455,21 +458,26 @@ controller_interface::CallbackReturn PidController::on_activate( controller_interface::return_type PidController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - auto current_ref = input_ref_.readFromRT(); + auto current_ref_op = input_ref_.try_get(); + if (current_ref_op.has_value()) + { + current_ref_ = current_ref_op.value(); + } for (size_t i = 0; i < dof_; ++i) { - if (!std::isnan((*current_ref)->values[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_[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]; + reference_interfaces_[dof_ + i] = current_ref_.values_dot[i]; } - - (*current_ref)->values[i] = std::numeric_limits::quiet_NaN(); + current_ref_.values[i] = std::numeric_limits::quiet_NaN(); } } + // save cleared input_ref_ + input_ref_.try_set(current_ref_); return controller_interface::return_type::OK; } @@ -482,13 +490,17 @@ controller_interface::return_type PidController::update_and_write_commands( // Update feedback either from external measured state or from state interfaces if (params_.use_external_measured_states) { - const auto measured_state = *(measured_state_.readFromRT()); + auto measured_state_op = measured_state_.try_get(); + if (measured_state_op.has_value()) + { + current_state_ = measured_state_op.value(); + } for (size_t i = 0; i < dof_; ++i) { - measured_state_values_[i] = measured_state->values[i]; + measured_state_values_[i] = current_state_.values[i]; if (measured_state_values_.size() == 2 * dof_) { - measured_state_values_[dof_ + i] = measured_state->values_dot[i]; + measured_state_values_[dof_ + i] = current_state_.values_dot[i]; } } } @@ -496,7 +508,15 @@ controller_interface::return_type PidController::update_and_write_commands( { for (size_t i = 0; i < measured_state_values_.size(); ++i) { - measured_state_values_[i] = state_interfaces_[i].get_value(); + const auto state_interface_value_op = state_interfaces_[i].get_optional(); + if (!state_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Unable to retrieve the state interface value for %s", + state_interfaces_[i].get_name().c_str()); + continue; + } + measured_state_values_[i] = state_interface_value_op.value(); } } @@ -514,11 +534,11 @@ controller_interface::return_type PidController::update_and_write_commands( if (std::isfinite(reference_interfaces_[i]) && std::isfinite(measured_state_values_[i])) { // calculate feed-forward - if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON) + if (*(feedforward_mode_enabled_.readFromRT())) { - // two interfaces if (reference_interfaces_.size() == 2 * dof_) { + // two interfaces if (std::isfinite(reference_interfaces_[dof_ + i])) { tmp_command = reference_interfaces_[dof_ + i] * @@ -574,36 +594,46 @@ controller_interface::return_type PidController::update_and_write_commands( } } - if (state_publisher_ && state_publisher_->trylock()) + if (state_publisher_) { - state_publisher_->msg_.header.stamp = time; + state_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]; + state_msg_.dof_states[i].reference = reference_interfaces_[i]; + state_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_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]; + state_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 = + state_msg_.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 = + state_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(); + state_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(); + const auto command_interface_value_op = command_interfaces_[i].get_optional(); + + if (!command_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Unable to retrieve the command interface value for %s", + command_interfaces_[i].get_name().c_str()); + } + else + { + state_msg_.dof_states[i].output = command_interface_value_op.value(); + } } - state_publisher_->unlockAndPublish(); + state_publisher_->try_publish(state_msg_); } return controller_interface::return_type::OK; diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 02448f2a19..46d7d3748a 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -43,6 +43,11 @@ pid_controller: default_value: false, description: "Use external states from a topic instead from state interfaces." } + enable_feedforward: { + type: bool, + default_value: false, + description: "Enables feedforward gain. (Will be deprecated in favour of setting feedforward_gain to a non-zero value.)" + } gains: __map_dof_names: p: { @@ -60,23 +65,59 @@ pid_controller: default_value: 0.0, description: "Derivative gain for PID" } + u_clamp_max: { + type: double, + default_value: .inf, + description: "Upper output clamp." + } + u_clamp_min: { + type: double, + default_value: -.inf, + description: "Lower output clamp." + } antiwindup: { type: bool, default_value: false, - description: "Antiwindup functionality. When set to true, limits + description: "[Deprecated, see antiwindup_strategy] Anti-windup 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." + default_value: .inf, + description: "[Deprecated, see antiwindup_strategy] Upper integral clamp." } i_clamp_min: { + type: double, + default_value: -.inf, + description: "[Deprecated, see antiwindup_strategy] Lower integral clamp." + } + antiwindup_strategy: { + type: string, + default_value: "legacy", + description: "Specifies the anti-windup strategy. Options: 'back_calculation', + 'conditional_integration', 'legacy' or 'none'. Note that the 'back_calculation' strategy use the + tracking_time_constant parameter to tune the anti-windup behavior.", + validation: { + one_of<>: [[ + "back_calculation", + "conditional_integration", + "legacy", + "none" + ]] + } + } + tracking_time_constant: { type: double, default_value: 0.0, - description: "Lower integral clamp." + description: "Specifies the tracking time constant for the 'back_calculation' strategy. If + set to 0.0 when this strategy is selected, a recommended default value will be applied." + } + error_deadband: { + type: double, + default_value: 1.e-16, + description: "Is used to stop integration when the error is within the given range." } feedforward_gain: { type: double, @@ -94,3 +135,8 @@ pid_controller: default_value: true, description: "Indicating if integral term is retained after re-activation" } + activate_state_publisher: { + type: bool, + default_value: false, + description: "Individual state publisher activation for each DOF. If true, the controller will publish the state of each DOF to the topic `///pid_state`." + } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index 15c4eaf049..87b0cb3176 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -1,3 +1,10 @@ +/**: + ros__parameters: +# TODO(christohfroehlich): Remove this global parameters once the deprecated antiwindup parameters are removed. + gains: + joint1: {antiwindup_strategy: "none",} + joint2: {antiwindup_strategy: "none",} + test_pid_controller: ros__parameters: dof_names: @@ -8,7 +15,19 @@ test_pid_controller: 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} + joint1: {p: 1.0, i: 2.0, d: 3.0, u_clamp_max: 5.0, u_clamp_min: -5.0} + +test_pid_controller_unlimited: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 1.0, i: 2.0, d: 3.0} test_pid_controller_angle_wraparound_on: ros__parameters: @@ -20,7 +39,7 @@ test_pid_controller_angle_wraparound_on: 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} + joint1: {p: 1.0, i: 2.0, d: 3.0, angle_wraparound: true} test_pid_controller_with_feedforward_gain: ros__parameters: @@ -32,7 +51,7 @@ test_pid_controller_with_feedforward_gain: 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} + joint1: {p: 0.5, i: 0.0, d: 0.0, feedforward_gain: 1.0} test_pid_controller_with_feedforward_gain_dual_interface: ros__parameters: @@ -45,8 +64,8 @@ test_pid_controller_with_feedforward_gain_dual_interface: 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} + joint1: {p: 0.5, i: 0.3, d: 0.4, feedforward_gain: 1.0} + joint2: {p: 0.5, i: 0.3, d: 0.4, feedforward_gain: 1.0} test_save_i_term_off: ros__parameters: @@ -58,7 +77,7 @@ test_save_i_term_off: 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, save_i_term: false} + joint1: {p: 1.0, i: 2.0, d: 3.0, save_i_term: false} test_save_i_term_on: ros__parameters: @@ -70,4 +89,4 @@ test_save_i_term_on: 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, save_i_term: true} + joint1: {p: 1.0, i: 2.0, d: 3.0, save_i_term: true} diff --git a/pid_controller/test/pid_controller_preceding_params.yaml b/pid_controller/test/pid_controller_preceding_params.yaml index 673abfe08e..b5ec89c2f5 100644 --- a/pid_controller/test/pid_controller_preceding_params.yaml +++ b/pid_controller/test/pid_controller_preceding_params.yaml @@ -9,3 +9,6 @@ test_pid_controller: reference_and_state_dof_names: - joint1state + + gains: + joint1: {antiwindup_strategy: "none"} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 1c0494a002..bd3fefcb60 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -21,8 +21,6 @@ #include #include -using pid_controller::feedforward_mode_type; - class PidControllerTest : public PidControllerFixture { }; @@ -48,8 +46,8 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) 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].u_clamp_max, 5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].u_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_); @@ -133,14 +131,14 @@ TEST_F(PidControllerTest, activate_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) + auto msg = controller_->input_ref_.get(); + 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_EQ(msg.values_dot.size(), dof_names_.size()); + for (const auto & cmd : msg.values_dot) { EXPECT_TRUE(std::isnan(cmd)); } @@ -181,15 +179,15 @@ TEST_F(PidControllerTest, reactivate_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_->command_interfaces_[0].get_optional().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_->command_interfaces_[0].get_optional().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_->command_interfaces_[0].get_optional().value(), 101.101); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -205,21 +203,68 @@ TEST_F(PidControllerTest, test_feedforward_mode_service) 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_->feedforward_mode_enabled_.readFromRT()), false); 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); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); // set to true ASSERT_NO_THROW(call_service(true, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); // set back to false ASSERT_NO_THROW(call_service(false, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); +} + +TEST_F(PidControllerTest, test_feedforward_mode_parameter) +{ + SetUpController(); + + // Check updating mode during on_configure + + // initially set to OFF + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + + // Reconfigure after setting parameter to true + ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS); + EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); + ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS); + EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful); + + // initially set to ON + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + + // Check updating mode during update_and_write_commands + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + + // Switch to ON + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); + EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", true}).successful); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); + + // Switch to OFF + EXPECT_TRUE(controller_->get_node()->set_parameter({"enable_feedforward", false}).successful); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); } /** @@ -229,7 +274,7 @@ TEST_F(PidControllerTest, test_feedforward_mode_service) TEST_F(PidControllerTest, test_update_logic_feedforward_off) { - SetUpController(); + SetUpController("test_pid_controller_unlimited"); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); executor.add_node(service_caller_node_->get_node_base_interface()); @@ -238,8 +283,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) 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); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().values[0])); + EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -249,8 +294,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) 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_FALSE(std::isnan(controller_->input_ref_.get().values[i])); + EXPECT_EQ(controller_->input_ref_.get().values[i], dof_command_values_[i]); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); } @@ -258,21 +303,24 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) 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_->feedforward_mode_enabled_.readFromRT()), false); 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])); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().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 OFF -> cmd = p_term + i_term + d_term = 30102.3 - const double expected_command_value = 30102.30102; + // p_term = 100.001 * 1, i_term = 0.0 at first update call, d_term = error/ds = 10000.1 * 3 + // feedforward ON, feedforward_gain = 0 + // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30100.3 + 0 * 101.101 = 30102.3 + const double expected_command_value = 30100.301000; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); } @@ -283,7 +331,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward_gain) { - SetUpController(); + SetUpController("test_pid_controller_unlimited"); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); executor.add_node(service_caller_node_->get_node_base_interface()); @@ -292,8 +340,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward 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); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().values[0])); + EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -301,13 +349,14 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward controller_->set_reference(dof_command_values_); - controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + controller_->get_node()->set_parameter(rclcpp::Parameter("enable_feedforward", true)); + controller_->feedforward_mode_enabled_.writeFromNonRT(true); + EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); 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_FALSE(std::isnan(controller_->input_ref_.get().values[i])); + EXPECT_EQ(controller_->input_ref_.get().values[i], dof_command_values_[i]); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); } @@ -315,23 +364,24 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward 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_->feedforward_mode_enabled_.readFromRT()), true); 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])); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().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 + // p_term = 100.001 * 1, i_term = 0.0 at first update call, 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; + // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30100.3 + 0 * 101.101 = 30102.3 + const double expected_command_value = 30100.301000; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); } } @@ -344,7 +394,7 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) { - SetUpController(); + SetUpController("test_pid_controller_unlimited"); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); executor.add_node(service_caller_node_->get_node_base_interface()); @@ -355,7 +405,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) 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); + EXPECT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); // update reference interface which will be used for calculation const double ref_interface_value = 5.0; @@ -383,12 +433,12 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) // 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, + // i_term = zero at first update // 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; + const double expected_command_value = 1173.9; - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } /** @@ -396,7 +446,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update) */ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) { - SetUpController(); + SetUpController("test_pid_controller_unlimited"); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); executor.add_node(service_caller_node_->get_node_base_interface()); @@ -405,7 +455,7 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) 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 + // write reference interface so that the values would be wrapped controller_->reference_interfaces_[0] = 10.0; // run update @@ -414,8 +464,15 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) 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); + // ref = 10.0, state = 1.1, ds = 0.01, p_gain = 1.0, i_gain = 2.0, d_gain = 3.0 + // error = ref - state = 10.0 - 1.1 = 8.9, error_dot = error/ds = 8.9/0.01 = 890.0, + // p_term = error * p_gain = 8.9 * 1.0 = 8.9, + // i_term = zero at first update + // d_term = error_dot * d_gain = 890.0 * 3.0 = 2670.0 + // feedforward OFF -> cmd = p_term + i_term + d_term = 8.9 + 0.0 + 2670.0 = 2678.9 + const double expected_command_value = 2678.9; + EXPECT_NEAR( + controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); } /** @@ -444,9 +501,15 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) 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); + // Check the command value with wrapped error + // ref = 10.0, state = 1.1, ds = 0.01, p_gain = 1.0, i_gain = 2.0, d_gain = 3.0 + // error = ref - state = wrap(10.0 - 1.1) = 8.9-2*pi = 2.616814, error_dot = error/ds + // = 2.6168/0.01 = 261.6814, p_term = error * p_gain = 2.6168 * 1.0 = 2.6168, i_term = zero at + // first update d_term = error_dot * d_gain = 261.6814 * 3.0 = 785.0444079 feedforward OFF -> cmd + // = p_term + i_term + d_term = 2.616814, + 0.0 + 785.0444079 = 787.6612219 + const double expected_command_value = 787.6612219; + EXPECT_NEAR( + controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5); } TEST_F(PidControllerTest, subscribe_and_get_messages_success) @@ -541,6 +604,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) const double expected_command_value = 6.95; SetUpController("test_pid_controller_with_feedforward_gain"); + controller_->get_node()->set_parameter(rclcpp::Parameter("enable_feedforward", true)); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); // check on interfaces & pid gain parameters @@ -567,8 +631,8 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) 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); + controller_->feedforward_mode_enabled_.writeFromNonRT(true); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); // send a message to update reference interface controller_->set_reference({target_value}); @@ -583,7 +647,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain) controller_interface::return_type::OK); // check on result from update - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } /** @@ -626,7 +690,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) ASSERT_TRUE(controller_->is_in_chained_mode()); // feedforward by default is OFF - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), false); // send a message to update reference interface controller_->set_reference({target_value}); @@ -641,7 +705,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain) controller_interface::return_type::OK); // check on result from update - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value); + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value); } /** @@ -673,11 +737,12 @@ TEST_F(PidControllerTest, test_save_i_term_off) // 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; + // p_term = 100.001 * 1, i_term = zero at first update, d_term = error/ds = 10000.1 * 3 + // feedforward OFF -> cmd = p_term + i_term + d_term = 30100.301 + const double expected_command_value = 30100.3010; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state @@ -690,7 +755,7 @@ TEST_F(PidControllerTest, test_save_i_term_off) ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, 0.0, 1e-5); } @@ -723,11 +788,12 @@ TEST_F(PidControllerTest, test_save_i_term_on) // 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; + // p_term = 100.001 * 1, i_term = zero at first update, d_term = error/ds = 10000.1 * 3 + // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.301 + const double expected_command_value = 30100.3010; - double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + double actual_value = + std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, expected_command_value, 1e-5); // deactivate the controller and set command=state @@ -740,7 +806,7 @@ TEST_F(PidControllerTest, test_save_i_term_on) ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5; + actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5; EXPECT_NEAR(actual_value, 2.00002, 1e-5); // i_term from above } diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 87b9e1de9a..d63fb5a322 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -54,6 +54,7 @@ class TestablePidController : public pid_controller::PidController FRIEND_TEST(PidControllerTest, activate_success); FRIEND_TEST(PidControllerTest, reactivate_success); FRIEND_TEST(PidControllerTest, test_feedforward_mode_service); + FRIEND_TEST(PidControllerTest, test_feedforward_mode_parameter); 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); @@ -108,15 +109,15 @@ class TestablePidController : public pid_controller::PidController 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) + ControllerCommandMsg msg; + 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[i] = target_value[i]; } - msg->values_dot.resize(msg->dof_names.size(), std::numeric_limits::quiet_NaN()); - input_ref_.writeFromNonRT(msg); + msg.values_dot.resize(msg.dof_names.size(), std::numeric_limits::quiet_NaN()); + input_ref_.set(msg); } }; @@ -140,12 +141,12 @@ class PidControllerFixture : public ::testing::Test 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"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_pid_controller/reference", rclcpp::SystemDefaultsQoS()); } static void TearDownTestCase() { rclcpp::shutdown(); } @@ -155,6 +156,9 @@ class PidControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_pid_controller") { + command_publisher_ = command_publisher_node_->create_publisher( + "/" + controller_name + "/reference", rclcpp::SystemDefaultsQoS()); + ASSERT_EQ( controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), controller_interface::return_type::OK); diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp index e006986473..9099d11a75 100644 --- a/pid_controller/test/test_pid_controller_dual_interface.cpp +++ b/pid_controller/test/test_pid_controller_dual_interface.cpp @@ -20,8 +20,6 @@ #include #include -using pid_controller::feedforward_mode_type; - class PidControllerDualInterfaceTest : public PidControllerFixture { public: @@ -58,6 +56,7 @@ class PidControllerDualInterfaceTest : public PidControllerFixtureget_node()->set_parameter(rclcpp::Parameter("enable_feedforward", true)); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); // check on interfaces & pid gain parameters @@ -80,8 +79,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i 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); + controller_->feedforward_mode_enabled_.writeFromNonRT(true); + ASSERT_EQ(*(controller_->feedforward_mode_enabled_.readFromRT()), true); // set up the reference interface, controller_->reference_interfaces_ = { @@ -94,10 +93,10 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i 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); + const double joint1_expected_cmd = 8.90; + const double joint2_expected_cmd = 9.90; + ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), joint1_expected_cmd); + ASSERT_EQ(controller_->command_interfaces_[1].get_optional().value(), joint2_expected_cmd); } int main(int argc, char ** argv) diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 602303ef9e..31629ed7b7 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -21,8 +21,6 @@ #include #include -using pid_controller::feedforward_mode_type; - class PidControllerTest : public PidControllerFixture { }; diff --git a/pose_broadcaster/CHANGELOG.rst b/pose_broadcaster/CHANGELOG.rst index ccc11ff790..1284166204 100644 --- a/pose_broadcaster/CHANGELOG.rst +++ b/pose_broadcaster/CHANGELOG.rst @@ -2,6 +2,46 @@ Changelog for package pose_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Deprecate tf.publish_rate parameter for pose_broadcaster (`#1614 `_) diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt index de4408ae26..fd3254b96d 100644 --- a/pose_broadcaster/CMakeLists.txt +++ b/pose_broadcaster/CMakeLists.txt @@ -38,11 +38,15 @@ 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} -) + pose_broadcaster_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${tf2_msgs_TARGETS} + ${geometry_msgs_TARGETS}) pluginlib_export_plugin_description_file( controller_interface pose_broadcaster.xml @@ -60,10 +64,8 @@ if(BUILD_TESTING) ) target_link_libraries(test_load_pose_broadcaster pose_broadcaster - ) - ament_target_dependencies(test_load_pose_broadcaster - controller_manager - ros2_control_test_assets + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) add_rostest_with_parameters_gmock(test_pose_broadcaster @@ -73,9 +75,6 @@ if(BUILD_TESTING) target_link_libraries(test_pose_broadcaster pose_broadcaster ) - ament_target_dependencies(test_pose_broadcaster - hardware_interface - ) endif() install( diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index 45aaa7a1ff..b9bf273518 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -62,6 +62,7 @@ class PoseBroadcaster : public controller_interface::ControllerInterface rclcpp::Publisher::SharedPtr pose_publisher_; std::unique_ptr> realtime_publisher_; + geometry_msgs::msg::PoseStamped pose_msg_; // TODO(amronos): Remove these two member variables std::optional tf_publish_period_; @@ -70,6 +71,7 @@ class PoseBroadcaster : public controller_interface::ControllerInterface rclcpp::Publisher::SharedPtr tf_publisher_; std::unique_ptr> realtime_tf_publisher_; + tf2_msgs::msg::TFMessage tf_msg_; }; } // namespace pose_broadcaster diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index d7ac7a9dd7..9065ffb923 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -2,7 +2,7 @@ pose_broadcaster - 4.24.0 + 4.33.1 Broadcaster to publish cartesian states. Bence Magyar diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index ef371c1d44..6d5f773d63 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -120,17 +120,13 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( } // Initialize pose message - realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_publisher_->unlock(); + pose_msg_.header.frame_id = params_.frame_id; // 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_msg_.transforms.resize(1); + auto & tf_transform = tf_msg_.transforms.front(); tf_transform.header.frame_id = params_.frame_id; if (params_.tf.child_frame_id.empty()) { @@ -140,8 +136,6 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( { tf_transform.child_frame_id = params_.tf.child_frame_id; } - - realtime_tf_publisher_->unlock(); } return controller_interface::CallbackReturn::SUCCESS; @@ -167,11 +161,11 @@ controller_interface::return_type PoseBroadcaster::update( geometry_msgs::msg::Pose pose; pose_sensor_->get_values_as_message(pose); - if (realtime_publisher_ && realtime_publisher_->trylock()) + if (realtime_publisher_) { - realtime_publisher_->msg_.header.stamp = time; - realtime_publisher_->msg_.pose = pose; - realtime_publisher_->unlockAndPublish(); + pose_msg_.header.stamp = time; + pose_msg_.pose = pose; + realtime_publisher_->try_publish(pose_msg_); } if (!is_pose_valid(pose)) { @@ -182,7 +176,7 @@ controller_interface::return_type PoseBroadcaster::update( pose.orientation.w); } // TODO(amronos): Remove publish rate functionality - else if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) + else if (realtime_tf_publisher_) { bool do_publish = false; // rlcpp::Time comparisons throw if clock types are not the same @@ -197,7 +191,7 @@ controller_interface::return_type PoseBroadcaster::update( if (do_publish) { - auto & tf_transform = realtime_tf_publisher_->msg_.transforms[0]; + auto & tf_transform = tf_msg_.transforms[0]; tf_transform.header.stamp = time; tf_transform.transform.translation.x = pose.position.x; @@ -209,14 +203,10 @@ controller_interface::return_type PoseBroadcaster::update( tf_transform.transform.rotation.z = pose.orientation.z; tf_transform.transform.rotation.w = pose.orientation.w; - realtime_tf_publisher_->unlockAndPublish(); + realtime_tf_publisher_->try_publish(tf_msg_); tf_last_publish_time_ = time; } - else - { - realtime_tf_publisher_->unlock(); - } } return controller_interface::return_type::OK; diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index fbb0a227be..283f50f665 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Call `configure()` of base class instead of node (`#1659 `_) diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index a49f073348..fa103c096d 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -25,7 +25,10 @@ target_include_directories(position_controllers PUBLIC $ $ ) -ament_target_dependencies(position_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(position_controllers PUBLIC + forward_command_controller::forward_command_controller + pluginlib::pluginlib + rclcpp::rclcpp) pluginlib_export_plugin_description_file(controller_interface position_controllers_plugins.xml) @@ -39,10 +42,8 @@ if(BUILD_TESTING) ) 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 + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) ament_add_gmock(test_joint_group_position_controller diff --git a/position_controllers/package.xml b/position_controllers/package.xml index aa132074c9..a15e07057d 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.24.0 + 4.33.1 Generic position controller for forwarding position commands. Bence Magyar diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index ea7005c543..3d38671cba 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -106,14 +106,14 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -121,9 +121,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); } TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) @@ -133,9 +133,9 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ( @@ -143,9 +143,9 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) @@ -160,9 +160,9 @@ TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) @@ -171,9 +171,9 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -204,7 +204,7 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0); } diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 11da4e0d3a..84af9bfbf0 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,51 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- +* Fix temporary copies of other semantic components (backport `#1905 `_) (`#1908 `_) +* Contributors: mergify[bot] + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- +* Fix cmake deprecation (backport `#1780 `_) (`#1782 `_) +* Contributors: mergify[bot] + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Remove CMAKE_CXX_STANDARD (backport `#1704 `_) (`#1706 `_) +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt index cdc63f2751..214e2c6fa9 100644 --- a/range_sensor_broadcaster/CMakeLists.txt +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -1,12 +1,6 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.16) 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() - find_package(ros2_control_cmake REQUIRED) set_compiler_options() export_windows_symbols() @@ -42,10 +36,15 @@ target_include_directories(range_sensor_broadcaster PRIVATE $ PRIVATE $ ) -ament_target_dependencies(range_sensor_broadcaster ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(range_sensor_broadcaster - range_sensor_broadcaster_parameters -) +target_link_libraries(range_sensor_broadcaster PUBLIC + range_sensor_broadcaster_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${sensor_msgs_TARGETS}) pluginlib_export_plugin_description_file( controller_interface range_sensor_broadcaster.xml) @@ -61,11 +60,9 @@ if(BUILD_TESTING) 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 + controller_manager::controller_manager + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets ) add_rostest_with_parameters_gmock(test_range_sensor_broadcaster @@ -75,9 +72,6 @@ if(BUILD_TESTING) target_link_libraries(test_range_sensor_broadcaster range_sensor_broadcaster ) - ament_target_dependencies(test_range_sensor_broadcaster - hardware_interface - ) endif() install( 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 index 65a1d3092f..75862d0815 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -60,6 +60,7 @@ class RangeSensorBroadcaster : public controller_interface::ControllerInterface using StatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr sensor_state_publisher_; std::unique_ptr realtime_publisher_; + sensor_msgs::msg::Range range_msg_; }; } // namespace range_sensor_broadcaster diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 6f627c5708..66f0837546 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.24.0 + 4.33.1 Controller to publish readings of range sensors. Bence Magyar diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp index 4ff817b2d3..bd9018e9d0 100644 --- a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp @@ -56,8 +56,7 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure( return CallbackReturn::ERROR; } - range_sensor_ = std::make_unique( - semantic_components::RangeSensor(params_.sensor_name)); + range_sensor_ = std::make_unique(params_.sensor_name); try { // register ft sensor data publisher @@ -73,17 +72,15 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure( return CallbackReturn::ERROR; } - realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_publisher_->msg_.radiation_type = static_cast(params_.radiation_type); - realtime_publisher_->msg_.field_of_view = static_cast(params_.field_of_view); - realtime_publisher_->msg_.min_range = static_cast(params_.min_range); - realtime_publisher_->msg_.max_range = static_cast(params_.max_range); + range_msg_.header.frame_id = params_.frame_id; + range_msg_.radiation_type = static_cast(params_.radiation_type); + range_msg_.field_of_view = static_cast(params_.field_of_view); + range_msg_.min_range = static_cast(params_.min_range); + range_msg_.max_range = static_cast(params_.max_range); // \note The versions conditioning is added here to support the source-compatibility with Humble #if SENSOR_MSGS_VERSION_MAJOR >= 5 - realtime_publisher_->msg_.variance = params_.variance; + range_msg_.variance = params_.variance; #endif - realtime_publisher_->unlock(); RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; @@ -123,11 +120,11 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_deactivate( controller_interface::return_type RangeSensorBroadcaster::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - if (realtime_publisher_ && realtime_publisher_->trylock()) + if (realtime_publisher_) { - realtime_publisher_->msg_.header.stamp = time; - range_sensor_->get_values_as_message(realtime_publisher_->msg_); - realtime_publisher_->unlockAndPublish(); + range_msg_.header.stamp = time; + range_sensor_->get_values_as_message(range_msg_); + realtime_publisher_->try_publish(range_msg_); } return controller_interface::return_type::OK; diff --git a/ros2_controllers-not-released.kilted.repos b/ros2_controllers-not-released.kilted.repos new file mode 100644 index 0000000000..1b3910e7e7 --- /dev/null +++ b/ros2_controllers-not-released.kilted.repos @@ -0,0 +1,6 @@ +repositories: + ## EXAMPLE DEPENDENCY +# : +# type: git +# url: git@github.com:/.git +# version: master diff --git a/ros2_controllers.jazzy.repos b/ros2_controllers.jazzy.repos index d4db80a0ae..c1f9472e57 100644 --- a/ros2_controllers.jazzy.repos +++ b/ros2_controllers.jazzy.repos @@ -2,7 +2,7 @@ repositories: ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git - version: master + version: jazzy realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git diff --git a/ros2_controllers.kilted.repos b/ros2_controllers.kilted.repos new file mode 100644 index 0000000000..4953c9faab --- /dev/null +++ b/ros2_controllers.kilted.repos @@ -0,0 +1,25 @@ +repositories: + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: kilted + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: kilted + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: master + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: kilted + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master + ros2_control_cmake: + type: git + url: https://github.com/ros-controls/ros2_control_cmake.git + version: master diff --git a/ros2_controllers.rolling.repos b/ros2_controllers.rolling.repos index f6462c4b4f..067271620f 100644 --- a/ros2_controllers.rolling.repos +++ b/ros2_controllers.rolling.repos @@ -14,7 +14,7 @@ repositories: control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git - version: ros2-master + version: master kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index f9cacf45e6..51187d639b 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,46 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- +* Add a generic chained_filter_controller (backport `#1634 `_) (`#1856 `_) +* Contributors: mergify[bot] + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- +* Add omni_wheel_drive_controller (backport `#1535 `_) (`#1836 `_) +* Contributors: mergify[bot] + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- diff --git a/ros2_controllers/doc/index.rst b/ros2_controllers/doc/index.rst index f785112944..f92d17702d 100644 --- a/ros2_controllers/doc/index.rst +++ b/ros2_controllers/doc/index.rst @@ -50,6 +50,10 @@ For more information of the ros2_control framework see `control.ros.org `__ - `API `__ - `ROS Index `__ + * - omni_wheel_drive_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ * - pid_controller - `Documentation `__ - `API `__ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index a9cb03e85c..21e2e21123 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.24.0 + 4.33.1 Metapackage for ros2_controllers related packages Bence Magyar @@ -20,6 +20,7 @@ ackermann_steering_controller admittance_controller bicycle_steering_controller + chained_filter_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster @@ -31,6 +32,7 @@ joint_state_broadcaster joint_trajectory_controller mecanum_drive_controller + omni_wheel_drive_controller parallel_gripper_controller pid_controller pose_broadcaster diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 2a0be0f257..43b2688966 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,44 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- + +4.25.0 (2025-05-17) +------------------- +* Use `warning` attribute of RcutilsLogger (backport `#1690 `_) (`#1691 `_) +* Contributors: mergify[bot], Sai Kishor Kothakota + 4.24.0 (2025-04-27) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 3b7a6f776d..49f4ca6817 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.24.0 + 4.33.1 Demo nodes for showing and testing functionalities of the ros2_control framework. Bence Magyar 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 cf38890407..47013a11cb 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 @@ -111,7 +111,7 @@ def get_sub_param(sub_param): self.get_logger().info(f'Goal "{name}" has definition {point}') else: - self.get_logger().warn( + self.get_logger().warning( f'Goal "{name}" definition is wrong. This goal will not be used. ' "Use the following structure: \n:\n " "positions: [joint1, joint2, joint3, ...]\n " @@ -150,11 +150,11 @@ def timer_callback(self): self.i %= len(self.goals) elif self.check_starting_point and not self.joint_state_msg_received: - self.get_logger().warn( + self.get_logger().warning( 'Start configuration could not be checked! Check "joint_state" topic!' ) else: - self.get_logger().warn("Start configuration is not within configured limits!") + self.get_logger().warning("Start configuration is not within configured limits!") def joint_state_callback(self, msg): @@ -166,7 +166,7 @@ def joint_state_callback(self, msg): if (msg.position[idx] < self.starting_point[enum][0]) or ( msg.position[idx] > self.starting_point[enum][1] ): - self.get_logger().warn(f"Starting point limits exceeded for joint {enum} !") + self.get_logger().warning(f"Starting point limits exceeded for joint {enum} !") limit_exceeded[idx] = True if any(limit_exceeded): diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index a23a614bfe..6316963a17 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.24.0", + version="4.33.1", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/ros_controls.jazzy.repos b/ros_controls.jazzy.repos index e7cfd385f8..4da12a2b37 100644 --- a/ros_controls.jazzy.repos +++ b/ros_controls.jazzy.repos @@ -6,4 +6,4 @@ repositories: ros-controls/ros2_control_demos: type: git url: https://github.com/ros-controls/ros2_control_demos.git - version: master + version: jazzy diff --git a/ros_controls.kilted.repos b/ros_controls.kilted.repos new file mode 100644 index 0000000000..da64a510fd --- /dev/null +++ b/ros_controls.kilted.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 b6960a2a7b..1d30144827 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- +* fix issue of not listing new JTCs (backport `#1891 `_) (`#1969 `_) +* Contributors: mergify[bot] + +4.33.0 (2025-10-03) +------------------- + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- +* fix rqt_joint_trajectory_controller for robots with namespace (backport `#1792 `_) (`#1803 `_) + Co-authored-by: Oscar Lima +* Contributors: mergify[bot] + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 14763a2ce7..f4589dceca 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.24.0 + 4.33.1 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar 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 e77c4410dd..1e5e80b7a9 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 @@ -71,55 +71,55 @@ def get_joint_limits(node, joints_names, use_smallest_joint_limits=True): continue name = child.getAttribute("name") - if name in joints_names: + try: + limit = child.getElementsByTagName("limit")[0] try: - 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: + minval = float(limit.getAttribute("lower")) + maxval = float(limit.getAttribute("upper")) + except ValueError: + if jtype == "continuous": + minval = -pi + maxval = pi + else: raise Exception( - f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" + f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" ) - except IndexError: + 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: + if name in joints_names: 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 - - 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 + 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 + + 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 12ac10c64c..38faed8bc6 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 @@ -464,7 +464,7 @@ def _jtc_joint_names(jtc_info): joint_names = [] for interface in jtc_info.required_state_interfaces: - name = interface.split("/")[-2] + name = "/".join(interface.split("/")[:-1]) if name not in joint_names: joint_names.append(name) diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py index 577cc1ab07..ec857b623d 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/utils.py @@ -152,6 +152,8 @@ def _srv_exists(node, srv_name, srv_type): srv_list = node.get_service_names_and_types() srv_info = [item for item in srv_list if item[0] == srv_name] + if len(srv_info) == 0: + return False srv_obtained_type = srv_info[0][1][0] return srv_type == srv_obtained_type diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 559fa53dc2..7e5d5f36e8 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.24.0", + version="4.33.1", 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 index 477b654066..47713ca4e6 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,53 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- +* Fix SteeringOdometry calculation error (backport `#1777 `_) (`#1779 `_) +* Contributors: mergify[bot] + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Fix steering_controllers_library docs (`#1734 `_) +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: Christoph Fröhlich, mergify[bot] + +4.25.0 (2025-05-17) +------------------- +* Simplify `on_set_chained_mode` avoiding cpplint warnings (backport `#1564 `_) (`#1688 `_) +* Contributors: mergify[bot], Bhagyesh Agresar + 4.24.0 (2025-04-27) ------------------- * Rename ackermann msg to controller state msg type (`#1662 `_) diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 9599e01d55..d57b00d278 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -46,8 +46,20 @@ 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}) + steering_controllers_library_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs + ${tf2_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${control_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${std_srvs_TARGETS}) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) @@ -59,11 +71,7 @@ if(BUILD_TESTING) ${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) diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 487cf94bb9..1aac29d543 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -64,7 +64,7 @@ Command interfaces ,,,,,,,,,,,,,,,,,,, - ``/position`` double, in rad -- ``/velocity`` double, in m/s +- ``/velocity`` double, in rad/s State interfaces ,,,,,,,,,,,,,,,,, @@ -77,7 +77,7 @@ Depending on the ``position_feedback``, different feedback types are expected With the following state interfaces: - ``/position`` double, in rad -- ``/`` double, in m or m/s +- ``/`` double, in rad or rad/s Subscribers ,,,,,,,,,,,, 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 index e700767e4c..6deabe8237 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -23,8 +23,8 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" // TODO(anyone): Replace with controller specific messages #include "control_msgs/msg/steering_controller_status.hpp" @@ -81,11 +81,14 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl std::shared_ptr param_listener_; steering_controllers_library::Params params_; - // Command subscribers and Controller State publisher - rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; - realtime_tools::RealtimeBuffer> input_ref_; + // the RT Box containing the command message + realtime_tools::RealtimeThreadSafeBox input_ref_; + // save the last reference in case of unable to get value from box + ControllerTwistReferenceMsg current_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; @@ -93,7 +96,9 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; std::unique_ptr rt_odom_state_publisher_; + ControllerStateMsgOdom odom_state_msg_; std::unique_ptr rt_tf_odom_state_publisher_; + ControllerStateMsgTf tf_odom_state_msg_; // override methods from ChainableControllerInterface std::vector on_export_reference_interfaces() override; @@ -108,6 +113,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl using ControllerStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr controller_s_publisher_; std::unique_ptr controller_state_publisher_; + SteeringControllerStateMsg controller_state_msg_; // name constants for state interfaces size_t nr_state_itfs_; diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index ed112d1086..5ec5cb8566 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.24.0 + 4.33.1 Package for steering robot configurations including odometry and interfaces. Bence Magyar diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 3840e2803b..ace7bc2611 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -31,16 +31,15 @@ using ControllerTwistReferenceMsg = // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, - const std::shared_ptr & node) + ControllerTwistReferenceMsg & 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(); + 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 @@ -333,10 +332,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( "~/reference", subscribers_qos, std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); - std::shared_ptr msg = - std::make_shared(); - reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.set(current_ref_); try { @@ -353,13 +350,12 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( 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; + odom_state_msg_.header.stamp = get_node()->now(); + odom_state_msg_.header.frame_id = params_.odom_frame_id; + odom_state_msg_.child_frame_id = params_.base_frame_id; + odom_state_msg_.pose.pose.position.z = 0; - auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + auto & covariance = odom_state_msg_.twist.covariance; constexpr size_t NUM_DIMENSIONS = 6; for (size_t index = 0; index < 6; ++index) { @@ -368,7 +364,6 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } - rt_odom_state_publisher_->unlock(); try { @@ -386,13 +381,11 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( 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(); + tf_odom_state_msg_.transforms.resize(1); + tf_odom_state_msg_.transforms[0].header.stamp = get_node()->now(); + tf_odom_state_msg_.transforms[0].header.frame_id = params_.odom_frame_id; + tf_odom_state_msg_.transforms[0].child_frame_id = params_.base_frame_id; + tf_odom_state_msg_.transforms[0].transform.translation.z = 0.0; try { @@ -410,10 +403,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( 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(); + controller_state_msg_.header.stamp = get_node()->now(); + controller_state_msg_.header.frame_id = params_.odom_frame_id; RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -433,7 +424,7 @@ void SteeringControllersLibrary::reference_callback( if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) { - input_ref_.writeFromNonRT(msg); + input_ref_.set(*msg); } else { @@ -513,17 +504,15 @@ SteeringControllersLibrary::on_export_reference_interfaces() return reference_interfaces; } -bool SteeringControllersLibrary::on_set_chained_mode(bool chained_mode) -{ - // Always accept switch to/from chained mode - return true || chained_mode; -} +bool SteeringControllersLibrary::on_set_chained_mode(bool /*chained_mode*/) { return true; } 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()); + // Try to set default value in command. + // If this fails, then another command will be received soon anyways. + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.try_set(current_ref_); return controller_interface::CallbackReturn::SUCCESS; } @@ -533,20 +522,58 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( { for (size_t i = 0; i < nr_cmd_itfs_; ++i) { - command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + if (!command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN())) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Failed to set NaN value for command interface '%s' (index %zu) during deactivation.", + command_interfaces_[i].get_name().c_str(), i); + return controller_interface::CallbackReturn::SUCCESS; + } } return controller_interface::CallbackReturn::SUCCESS; } controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto current_ref = *(input_ref_.readFromRT()); + auto current_ref_op = input_ref_.try_get(); + if (current_ref_op.has_value()) + { + current_ref_ = current_ref_op.value(); + } + + const auto age_of_last_command = time - current_ref_.header.stamp; - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + // accept message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; + if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.linear.y)) + { + reference_interfaces_[0] = current_ref_.twist.linear.x; + reference_interfaces_[1] = 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.angular.z = std::numeric_limits::quiet_NaN(); + + input_ref_.try_set(current_ref_); + } + } + } + else + { + if (!std::isnan(current_ref_.twist.linear.x) && !std::isnan(current_ref_.twist.angular.z)) + { + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + + current_ref_.twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref_.twist.angular.z = std::numeric_limits::quiet_NaN(); + + input_ref_.try_set(current_ref_); + } } return controller_interface::return_type::OK; @@ -556,6 +583,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c const rclcpp::Time & time, const rclcpp::Duration & period) { update_odometry(period); + auto logger = get_node()->get_logger(); // MOVE ROBOT @@ -564,25 +592,36 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c 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); for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { - command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); + if (!command_interfaces_[i].set_value(traction_commands[i])) + { + RCLCPP_WARN( + logger, "Unable to set traction command at index %zu: value = %f", i, + traction_commands[i]); + return controller_interface::return_type::OK; + } } for (size_t i = 0; i < params_.steering_joints_names.size(); i++) { - command_interfaces_[i + params_.traction_joints_names.size()].set_value(steering_commands[i]); + const auto & value = steering_commands[i]; + + if (!command_interfaces_[i + params_.traction_joints_names.size()].set_value(value)) + { + RCLCPP_WARN(logger, "Unable to set steering command at index %zu: value = %f", i, value); + return controller_interface::return_type::OK; + } + } + } + else + { + for (size_t i = 0; i < params_.traction_joints_names.size(); i++) + { + command_interfaces_[i].set_value(0.0); } } @@ -592,38 +631,35 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c orientation.setRPY(0.0, 0.0, odometry_.get_heading()); // Populate odom message and publish - if (rt_odom_state_publisher_->trylock()) + if (rt_odom_state_publisher_) { - 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(); + odom_state_msg_.header.stamp = time; + odom_state_msg_.pose.pose.position.x = odometry_.get_x(); + odom_state_msg_.pose.pose.position.y = odometry_.get_y(); + odom_state_msg_.pose.pose.orientation = tf2::toMsg(orientation); + odom_state_msg_.twist.twist.linear.x = odometry_.get_linear(); + odom_state_msg_.twist.twist.angular.z = odometry_.get_angular(); + rt_odom_state_publisher_->try_publish(odom_state_msg_); } // Publish tf /odom frame - if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_) { - 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(); + tf_odom_state_msg_.transforms.front().header.stamp = time; + tf_odom_state_msg_.transforms.front().transform.translation.x = odometry_.get_x(); + tf_odom_state_msg_.transforms.front().transform.translation.y = odometry_.get_y(); + tf_odom_state_msg_.transforms.front().transform.rotation = tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->try_publish(tf_odom_state_msg_); } - if (controller_state_publisher_->trylock()) + if (controller_state_publisher_) { - 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(); + controller_state_msg_.header.stamp = time; + controller_state_msg_.traction_wheels_position.clear(); + controller_state_msg_.traction_wheels_velocity.clear(); + controller_state_msg_.linear_velocity_command.clear(); + controller_state_msg_.steer_positions.clear(); + controller_state_msg_.steering_angle_command.clear(); auto number_of_traction_wheels = params_.traction_joints_names.size(); auto number_of_steering_wheels = params_.steering_joints_names.size(); @@ -632,27 +668,67 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { if (params_.position_feedback) { - controller_state_publisher_->msg_.traction_wheels_position.push_back( - state_interfaces_[i].get_value()); + auto position_state_interface_op = state_interfaces_[i].get_optional(); + if (!position_state_interface_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve position feedback data for traction wheel %zu", i); + } + else + { + controller_state_msg_.traction_wheels_position.push_back( + position_state_interface_op.value()); + } } else { - controller_state_publisher_->msg_.traction_wheels_velocity.push_back( - state_interfaces_[i].get_value()); + auto velocity_state_interface_op = state_interfaces_[i].get_optional(); + if (!velocity_state_interface_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve velocity feedback data for traction wheel %zu", i); + } + else + { + controller_state_msg_.traction_wheels_velocity.push_back( + velocity_state_interface_op.value()); + } + } + + auto velocity_command_interface_op = command_interfaces_[i].get_optional(); + if (!velocity_command_interface_op.has_value()) + { + RCLCPP_DEBUG(logger, "Unable to retrieve velocity command for traction wheel %zu", i); + } + else + { + controller_state_msg_.linear_velocity_command.push_back( + velocity_command_interface_op.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()); + const auto state_interface_value_op = + state_interfaces_[number_of_traction_wheels + i].get_optional(); + const auto command_interface_value_op = + command_interfaces_[number_of_traction_wheels + i].get_optional(); + if (!state_interface_value_op.has_value() || !command_interface_value_op.has_value()) + { + RCLCPP_DEBUG( + logger, "Unable to retrieve %s for steering wheel %zu", + !state_interface_value_op.has_value() ? "state interface value" + : "command interface value", + i); + } + else + { + controller_state_msg_.steer_positions.push_back(state_interface_value_op.value()); + controller_state_msg_.steering_angle_command.push_back(command_interface_value_op.value()); + } } - controller_state_publisher_->unlockAndPublish(); + controller_state_publisher_->try_publish(controller_state_msg_); } reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 5f480ca03d..0a5367f4bd 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -180,7 +180,7 @@ bool SteeringOdometry::update_from_velocity( 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 / wheel_base_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheel_base_; return update_odometry(linear_velocity, angular_velocity, dt); } diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 26750777c9..79740f3693 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -80,11 +80,12 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) } // Tests controller update_reference_from_subscribers and -// its two cases for position_feedback true/false behavior +// for position_feedback 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) +TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) { SetUpController(); + controller_->params_.position_feedback = true; rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -104,98 +105,180 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) const double TEST_LINEAR_VELOCITY_Y = 0.0; const double TEST_ANGULAR_VELOCITY_Z = 0.3; - std::shared_ptr msg = std::make_shared(); + ControllerReferenceMsg msg; + + msg.header.stamp = controller_->get_node()->now(); + 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_.set(msg); + + // age_of_last_command < ref_timeout_ + auto age_of_last_command = + controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + ASSERT_TRUE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_.get().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); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); + + // are the command_itfs updated? + EXPECT_NEAR(controller_->command_interfaces_[0].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[1].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); // 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; + 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_.set(msg); + + age_of_last_command = controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + + // 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_.set(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_->input_ref_.get().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); + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.angular.z)); + + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_optional().value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); +} + +// Tests controller update_reference_from_subscribers and +// for position_feedback=false behavior +// when too old msg is sent i.e age_of_last_command > ref_timeout case +TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) +{ + SetUpController(); + controller_->params_.position_feedback = false; + + 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()); - 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); + // 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; - // 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); + ControllerReferenceMsg msg; - // case 2 position_feedback = true - controller_->params_.position_feedback = true; + msg.header.stamp = controller_->get_node()->now(); + 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_.set(msg); - // 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); + auto age_of_last_command = + controller_->get_node()->now() - controller_->input_ref_.get().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); + // age_of_last_command < ref_timeout_ + ASSERT_TRUE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_.get().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); + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_FALSE(std::isnan(controller_->input_ref_.get().twist.angular.z)); + + // are the command_itfs updated? + EXPECT_NEAR(controller_->command_interfaces_[0].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[1].get_optional().value(), 3.333333, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); + + // 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_.set(msg); + + age_of_last_command = controller_->get_node()->now() - controller_->input_ref_.get().header.stamp; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ(controller_->input_ref_.get().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])); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); } + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); + ASSERT_TRUE(std::isnan(controller_->input_ref_.get().twist.angular.z)); // Wheel velocities should reset to 0 - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); - EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_optional().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); + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); } int main(int argc, char ** argv) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 22d70fd7a5..5aac580b33 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -70,7 +70,8 @@ class TestableSteeringControllersLibrary : public steering_controllers_library::SteeringControllersLibrary { FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces); - FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); + FRIEND_TEST(SteeringControllersLibraryTest, test_position_feedback_ref_timeout); + FRIEND_TEST(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout); public: controller_interface::CallbackReturn on_configure( diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 3f2ff8cf11..a3c4de3e97 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,52 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- +* Don't use `msg\_` field of realtime publisher (backport `#1947 `_) (`#1948 `_) +* Contributors: mergify[bot] + +4.33.0 (2025-10-03) +------------------- +* Update API for realtime publisher (backport `#1830 `_) (`#1944 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Fix: Remove deprecated `rclcpp::spin_some(node)` (backport `#1928 `_) (`#1932 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- +* Deprecating tf2 C Headers (`#1325 `_) +* Contributors: Lucas Wendland + 4.24.0 (2025-04-27) ------------------- * Call `configure()` of base class instead of node (`#1659 `_) diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 7192afa5cc..0c71042d63 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -46,8 +46,22 @@ target_include_directories(tricycle_controller PUBLIC $ $ ) -target_link_libraries(tricycle_controller PUBLIC tricycle_controller_parameters) -ament_target_dependencies(tricycle_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(tricycle_controller PUBLIC + tricycle_controller_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + tf2::tf2 + rcpputils::rcpputils + ${ackermann_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${tf2_msgs_TARGETS} + ${std_srvs_TARGETS} + ${builtin_interfaces_TARGETS}) pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) @@ -61,25 +75,15 @@ if(BUILD_TESTING) target_link_libraries(test_tricycle_controller tricycle_controller ) - ament_target_dependencies(test_tricycle_controller - geometry_msgs - hardware_interface - nav_msgs - rclcpp - rclcpp_lifecycle - realtime_tools - tf2 - tf2_msgs - ) add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") ament_add_gmock(test_load_tricycle_controller test/test_load_tricycle_controller.cpp) target_link_libraries(test_load_tricycle_controller tricycle_controller ) - ament_target_dependencies(test_load_tricycle_controller - controller_manager - ros2_control_test_assets + target_link_libraries(test_load_tricycle_controller + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) ament_add_gmock(test_traction_limiter diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 60590012b9..81a62b989c 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -33,8 +33,8 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_box.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -106,17 +106,20 @@ class TricycleController : public controller_interface::ControllerInterface std::shared_ptr> ackermann_command_publisher_ = nullptr; std::shared_ptr> realtime_ackermann_command_publisher_ = nullptr; + AckermannDrive ackermann_command_msg_; Odometry odometry_; std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> realtime_odometry_publisher_ = nullptr; + nav_msgs::msg::Odometry odometry_msg_; std::shared_ptr> odometry_transform_publisher_ = nullptr; std::shared_ptr> realtime_odometry_transform_publisher_ = nullptr; + tf2_msgs::msg::TFMessage tf_msg_; // Timeout to consider cmd_vel commands old std::chrono::milliseconds cmd_vel_timeout_{500}; @@ -124,7 +127,8 @@ class TricycleController : public controller_interface::ControllerInterface bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + realtime_tools::RealtimeThreadSafeBox> received_velocity_msg_ptr_{ + nullptr}; std::shared_ptr last_command_msg_; rclcpp::Service::SharedPtr reset_odom_service_; diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index b57aae052a..4256d8acdb 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.24.0 + 4.33.1 Controller for a tricycle drive mobile base Bence Magyar diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 6d32b8265a..3037a7e92c 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -110,8 +110,19 @@ controller_interface::return_type TricycleController::update( TwistStamped command = *last_command_msg_; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; - double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s - double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians + auto Ws_read_op = traction_joint_[0].velocity_state.get().get_optional(); + auto alpha_read_op = steering_joint_[0].position_state.get().get_optional(); + + if (!Ws_read_op.has_value() || !alpha_read_op.has_value()) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Unable to retrieve the data of traction joint velocity or steering joint position"); + return controller_interface::return_type::OK; + } + + double Ws_read = Ws_read_op.value(); // in radians/s + double alpha_read = alpha_read_op.value(); // in radians if (params_.open_loop) { @@ -130,27 +141,26 @@ controller_interface::return_type TricycleController::update( tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (realtime_odometry_publisher_->trylock()) + if (realtime_odometry_publisher_) { - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.stamp = time; + odometry_msg_.header.stamp = time; if (!params_.odom_only_twist) { - odometry_message.pose.pose.position.x = odometry_.getX(); - odometry_message.pose.pose.position.y = odometry_.getY(); - odometry_message.pose.pose.orientation.x = orientation.x(); - odometry_message.pose.pose.orientation.y = orientation.y(); - odometry_message.pose.pose.orientation.z = orientation.z(); - odometry_message.pose.pose.orientation.w = orientation.w(); + odometry_msg_.pose.pose.position.x = odometry_.getX(); + odometry_msg_.pose.pose.position.y = odometry_.getY(); + odometry_msg_.pose.pose.orientation.x = orientation.x(); + odometry_msg_.pose.pose.orientation.y = orientation.y(); + odometry_msg_.pose.pose.orientation.z = orientation.z(); + odometry_msg_.pose.pose.orientation.w = orientation.w(); } - odometry_message.twist.twist.linear.x = odometry_.getLinear(); - odometry_message.twist.twist.angular.z = odometry_.getAngular(); - realtime_odometry_publisher_->unlockAndPublish(); + odometry_msg_.twist.twist.linear.x = odometry_.getLinear(); + odometry_msg_.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->try_publish(odometry_msg_); } - if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_) { - auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + auto & transform = tf_msg_.transforms.front(); transform.header.stamp = time; transform.transform.translation.x = odometry_.getX(); transform.transform.translation.y = odometry_.getY(); @@ -158,7 +168,7 @@ controller_interface::return_type TricycleController::update( transform.transform.rotation.y = orientation.y(); transform.transform.rotation.z = orientation.z(); transform.transform.rotation.w = orientation.w(); - realtime_odometry_transform_publisher_->unlockAndPublish(); + realtime_odometry_transform_publisher_->try_publish(tf_msg_); } // Compute wheel velocity and angle @@ -201,18 +211,27 @@ controller_interface::return_type TricycleController::update( previous_commands_.emplace(ackermann_command); // Publish ackermann command - if (params_.publish_ackermann_command && realtime_ackermann_command_publisher_->trylock()) + if (params_.publish_ackermann_command && realtime_ackermann_command_publisher_) { - 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 = static_cast(Ws_write); - realtime_ackermann_command.steering_angle = static_cast(alpha_write); - realtime_ackermann_command_publisher_->unlockAndPublish(); + ackermann_command_msg_.speed = static_cast(Ws_write); + ackermann_command_msg_.steering_angle = static_cast(alpha_write); + realtime_ackermann_command_publisher_->try_publish(ackermann_command_msg_); } - traction_joint_[0].velocity_command.get().set_value(Ws_write); - steering_joint_[0].position_command.get().set_value(alpha_write); + if (!traction_joint_[0].velocity_command.get().set_value(Ws_write)) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Unable to set the velocity command for traction joint to value: '%f'.", Ws_write); + } + if (!steering_joint_[0].position_command.get().set_value(alpha_write)) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Unable to set the position command for steering joint to value: '%f'.", alpha_write); + } return controller_interface::return_type::OK; } @@ -312,12 +331,11 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & std::make_shared>( odometry_publisher_); - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = params_.odom_frame_id; - odometry_message.child_frame_id = params_.base_frame_id; + odometry_msg_.header.frame_id = params_.odom_frame_id; + odometry_msg_.child_frame_id = params_.base_frame_id; // initialize odom values zeros - odometry_message.twist = + odometry_msg_.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); constexpr size_t NUM_DIMENSIONS = 6; @@ -325,8 +343,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { // 0, 7, 14, 21, 28, 35 const size_t diagonal_index = NUM_DIMENSIONS * index + index; - odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; - odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + odometry_msg_.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_msg_.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } // initialize transform publisher and message @@ -339,10 +357,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odometry_transform_publisher_); // keeping track of odom and base_link transforms only - auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; - odometry_transform_message.transforms.resize(1); - odometry_transform_message.transforms.front().header.frame_id = params_.odom_frame_id; - odometry_transform_message.transforms.front().child_frame_id = params_.base_frame_id; + tf_msg_.transforms.resize(1); + tf_msg_.transforms.front().header.frame_id = params_.odom_frame_id; + tf_msg_.transforms.front().child_frame_id = params_.base_frame_id; } // Create odom reset service @@ -433,8 +450,18 @@ bool TricycleController::reset() void TricycleController::halt() { - traction_joint_[0].velocity_command.get().set_value(0.0); - steering_joint_[0].position_command.get().set_value(0.0); + if (!traction_joint_[0].velocity_command.get().set_value(0.0)) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Unable to set the velocity command for traction joint to value 0.0"); + } + if (!steering_joint_[0].position_command.get().set_value(0.0)) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Unable to set the position command for steering joint to value 0.0"); + } } CallbackReturn TricycleController::get_traction( diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 55f41801b5..a933ef1fa8 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -120,7 +120,7 @@ class TestTricycleController : public ::testing::Test } /// \brief wait for the subscriber and publisher to completely setup - void waitForSetup() + void waitForSetup(rclcpp::Executor & executor) { constexpr std::chrono::seconds TIMEOUT{2}; auto clock = pub_node->get_clock(); @@ -131,7 +131,8 @@ class TestTricycleController : public ::testing::Test { FAIL(); } - rclcpp::spin_some(pub_node); + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } } @@ -268,7 +269,7 @@ TEST_F(TestTricycleController, cleanup) state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - waitForSetup(); + waitForSetup(executor); // send msg const double linear = 1.0; @@ -284,15 +285,15 @@ TEST_F(TestTricycleController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); executor.cancel(); } @@ -312,8 +313,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) assignResources(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(position_, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_optional().value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -328,8 +329,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_optional().value()); // deactivated // wait so controller process the second point when deactivated @@ -340,14 +341,16 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()) + << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()) + << "Wheels are halted on deactivate()"; // cleanup state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value()); state = controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 9bffdc6314..1aea621a1d 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Rename ackermann msg to controller state msg type (`#1662 `_) diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index ef8901f674..22eb46bbea 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -40,8 +40,15 @@ 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}) + tricycle_steering_controller_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + steering_controllers_library::steering_controllers_library + ${std_srvs_TARGETS}) pluginlib_export_plugin_description_file( controller_interface tricycle_steering_controller.xml) @@ -54,10 +61,10 @@ if(BUILD_TESTING) add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") ament_add_gmock(test_load_tricycle_steering_controller test/test_load_tricycle_steering_controller.cpp) - ament_target_dependencies(test_load_tricycle_steering_controller - controller_manager - hardware_interface - ros2_control_test_assets + target_link_libraries(test_load_tricycle_steering_controller + controller_manager::controller_manager + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets ) add_rostest_with_parameters_gmock( @@ -65,22 +72,12 @@ if(BUILD_TESTING) ${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_preceding test/test_tricycle_steering_controller_preceding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_preceding_params.yaml) target_include_directories(test_tricycle_steering_controller_preceding PRIVATE include) target_link_libraries(test_tricycle_steering_controller_preceding tricycle_steering_controller) - ament_target_dependencies( - test_tricycle_steering_controller_preceding - controller_interface - hardware_interface - ) endif() install( diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index eb6be5b584..28bab30d9e 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.24.0 + 4.33.1 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Bence Magyar diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 2c919cbfd8..c626301284 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -86,11 +86,24 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period } 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(); + const auto traction_right_wheel_value_op = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional(); + const auto traction_left_wheel_value_op = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional(); + const auto steering_position_op = state_interfaces_[STATE_STEER_AXIS].get_optional(); + + if ( + !traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() || + !steering_position_op.has_value()) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Unable to retrieve the data from right wheel or left wheel or steering position"); + return true; + } + const double traction_right_wheel_value = traction_right_wheel_value_op.value(); + const double traction_left_wheel_value = traction_left_wheel_value_op.value(); + const double steering_position = steering_position_op.value(); if ( std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && std::isfinite(steering_position)) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 4f9b53e31f..8096fd980a 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -96,13 +96,13 @@ TEST_F(TricycleSteeringControllerTest, activate_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)); + auto msg = controller_->input_ref_.get(); + 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) @@ -133,9 +133,9 @@ TEST_F(TricycleSteeringControllerTest, reactivate_success) 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_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value())); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -154,27 +154,27 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) 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); + ControllerReferenceMsg msg; + msg.header.stamp = controller_->get_node()->now(); + msg.twist.linear.x = 0.1; + msg.twist.angular.z = 0.2; + controller_->input_ref_.set(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); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -202,16 +202,16 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) // 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); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan(controller_->input_ref_.get().twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -235,8 +235,9 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu 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); + // never received a valid command, linear velocity should have been reset + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.0); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 0.0); EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(); @@ -248,13 +249,13 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu // 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); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 1d0e64b281..2e29ecf7ce 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.33.1 (2025-10-17) +------------------- + +4.33.0 (2025-10-03) +------------------- +* Update realtime containers (backport `#1721 `_) (`#1935 `_) +* Use new handles API in ros2_controllers to fix deprecation warnings (backport `#1566 `_) (`#1934 `_) +* Contributors: mergify[bot] + +4.32.0 (2025-09-12) +------------------- + +4.31.0 (2025-08-27) +------------------- + +4.30.1 (2025-08-03) +------------------- + +4.30.0 (2025-07-31) +------------------- + +4.29.0 (2025-07-23) +------------------- + +4.28.0 (2025-07-14) +------------------- + +4.27.1 (2025-07-02) +------------------- + +4.27.0 (2025-06-23) +------------------- + +4.26.0 (2025-06-06) +------------------- +* Use target_link_libraries instead of ament_target_dependencies (backport `#1697 `_) (`#1699 `_) +* Contributors: mergify[bot] + +4.25.0 (2025-05-17) +------------------- + 4.24.0 (2025-04-27) ------------------- * Call `configure()` of base class instead of node (`#1659 `_) diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index dc89696b03..153a8c13ed 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -25,7 +25,10 @@ target_include_directories(velocity_controllers PUBLIC $ $ ) -ament_target_dependencies(velocity_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(velocity_controllers PUBLIC + forward_command_controller::forward_command_controller + pluginlib::pluginlib + rclcpp::rclcpp) pluginlib_export_plugin_description_file(controller_interface velocity_controllers_plugins.xml) @@ -39,10 +42,8 @@ if(BUILD_TESTING) ) 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 + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets ) ament_add_gmock(test_joint_group_velocity_controller diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index f0883b4920..60a3cf3308 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.24.0 + 4.33.1 Generic controller for forwarding commands. Bence Magyar diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index 97f6713d72..b2d0086254 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -59,7 +59,11 @@ controller_interface::CallbackReturn JointGroupVelocityController::on_deactivate // stop all joints for (auto & command_interface : command_interfaces_) { - command_interface.set_value(0.0); + if (!command_interface.set_value(0.0)) + { + RCLCPP_WARN(get_node()->get_logger(), "Unable to set the command interface to value 0.0"); + return controller_interface::CallbackReturn::SUCCESS; + } } return ret; diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 9a9af3fb81..9d7a5f2b19 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -106,14 +106,14 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); // send command - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0, 30.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0, 30.0}; + controller_->rt_command_.set(command); // update successful, command received ASSERT_EQ( @@ -121,9 +121,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) @@ -133,9 +133,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // send command with wrong number of joints - auto command_ptr = std::make_shared(); - command_ptr->data = {10.0, 20.0}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + forward_command_controller::CmdType command; + command.data = {10.0, 20.0}; + controller_->rt_command_.set(command); // update failed, command size does not match number of joints ASSERT_EQ( @@ -143,9 +143,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) @@ -160,9 +160,9 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) @@ -171,9 +171,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -204,9 +204,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) @@ -218,15 +218,15 @@ TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_.get_optional().value(), 0.0); + ASSERT_EQ(joint_2_cmd_.get_optional().value(), 0.0); + ASSERT_EQ(joint_3_cmd_.get_optional().value(), 0.0); }