diff --git a/.github/mergify.yml b/.github/mergify.yml index 0a6e425a30..39ee6b6bc0 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -32,13 +32,13 @@ pull_request_rules: - author=mergify[bot] actions: comment: - message: This pull request is in conflict. Could you fix it @bmagyar @dstogl @christophfroehlich? + message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich? - name: development targets master branch conditions: - base!=master - author!=bmagyar - - author!=dstogl + - author!=destogl - author!=christophfroehlich - author!=mergify[bot] - author!=dependabot[bot] @@ -46,5 +46,5 @@ pull_request_rules: comment: message: | @{{author}}, all pull requests must be targeted towards the `master` development branch. - Once merged into `master`, it is possible to backport to @{{base}}, but it must be in `master` + Once merged into `master`, it is possible to backport to `{{base}}`, but it must be in `master` to have these changes reflected into new distributions. diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml index a1d159541d..357ee3dfa8 100644 --- a/.github/workflows/ci-coverage-build-humble.yml +++ b/.github/workflows/ci-coverage-build-humble.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} @@ -55,12 +55,12 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.4 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.0.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-coverage-humble path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml index 08e6eafd82..7914a1acb0 100644 --- a/.github/workflows/ci-coverage-build-iron.yml +++ b/.github/workflows/ci-coverage-build-iron.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} @@ -55,12 +55,12 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.4 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.0.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-coverage-iron path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index f785652989..b96276ca5a 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} @@ -55,12 +55,12 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.4 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.0.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index f6b9c027c9..df17d11dc4 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -2,6 +2,30 @@ name: ROS Lint on: pull_request: +env: + package-name: + ackermann_steering_controller + admittance_controller + bicycle_steering_controller + diff_drive_controller + effort_controllers + force_torque_sensor_broadcaster + forward_command_controller + gripper_controllers + imu_sensor_broadcaster + joint_state_broadcaster + joint_trajectory_controller + pid_controller + position_controllers + range_sensor_broadcaster + ros2_controllers + ros2_controllers_test_nodes + rqt_joint_trajectory_controller + steering_controllers_library + tricycle_controller + tricycle_steering_controller + velocity_controllers + jobs: ament_lint: name: ament_${{ matrix.linter }} @@ -19,28 +43,7 @@ jobs: with: distribution: rolling linter: ${{ matrix.linter }} - package-name: - ackermann_steering_controller - admittance_controller - bicycle_steering_controller - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - gripper_controllers - imu_sensor_broadcaster - joint_state_broadcaster - joint_trajectory_controller - pid_controller - position_controllers - range_sensor_broadcaster - ros2_controllers - ros2_controllers_test_nodes - rqt_joint_trajectory_controller - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers + package-name: ${{ env.package-name }} ament_lint_100: @@ -58,25 +61,4 @@ jobs: distribution: rolling linter: cpplint arguments: "--linelength=100 --filter=-whitespace/newline" - package-name: - ackermann_steering_controller - admittance_controller - bicycle_steering_controller - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - gripper_controllers - imu_sensor_broadcaster - joint_state_broadcaster - joint_trajectory_controller - pid_controller - position_controllers - range_sensor_broadcaster - ros2_controllers - ros2_controllers_test_nodes - rqt_joint_trajectory_controller - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers + package-name: ${{ env.package-name }} diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/humble-binary-build-main.yml index 64d78f281a..9c634b372a 100644 --- a/.github/workflows/humble-binary-build-main.yml +++ b/.github/workflows/humble-binary-build-main.yml @@ -4,8 +4,6 @@ name: Humble Binary Build - main on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml index 524cacd685..b662543959 100644 --- a/.github/workflows/humble-binary-build-testing.yml +++ b/.github/workflows/humble-binary-build-testing.yml @@ -4,8 +4,6 @@ name: Humble Binary Build - testing on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml new file mode 100644 index 0000000000..c236aecf64 --- /dev/null +++ b/.github/workflows/humble-debian-build.yml @@ -0,0 +1,20 @@ +name: Debian Humble Source Build +on: + workflow_dispatch: + pull_request: + branches: + - humble + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + humble_debian: + name: Humble debian build + uses: ./.github/workflows/reusable-debian-build.yml + with: + ros_distro: humble + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: humble + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 9da6059892..db503d1993 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -1,33 +1,19 @@ -name: Humble RHEL Binary Build +name: RHEL Humble Binary Build on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble - push: - branches: - - humble schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' - jobs: humble_rhel_binary: name: Humble RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: humble - container: ghcr.io/ros-controls/ros:humble-rhel - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - - run: | - rosdep update - rosdep install -iy --from-path src/ros2_controllers - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test + uses: ./.github/workflows/reusable-rhel-binary-build.yml + with: + ros_distro: humble + upstream_workspace: ros2_controllers.humble.repos + ref_for_scheduled_build: humble + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml index 863df79a22..bfe83392ea 100644 --- a/.github/workflows/humble-semi-binary-build-main.yml +++ b/.github/workflows/humble-semi-binary-build-main.yml @@ -3,8 +3,6 @@ name: Humble Semi-Binary Build - main on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-semi-binary-build-testing.yml b/.github/workflows/humble-semi-binary-build-testing.yml index 6286636e1f..3a66c0b74d 100644 --- a/.github/workflows/humble-semi-binary-build-testing.yml +++ b/.github/workflows/humble-semi-binary-build-testing.yml @@ -3,8 +3,6 @@ name: Humble Semi-Binary Build - testing on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index ff0fd62e05..a40d53f8e3 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -1,8 +1,6 @@ name: Humble Source Build on: workflow_dispatch: - branches: - - humble push: branches: - humble diff --git a/.github/workflows/iron-binary-build-main.yml b/.github/workflows/iron-binary-build-main.yml index ef35397855..bb1997bd48 100644 --- a/.github/workflows/iron-binary-build-main.yml +++ b/.github/workflows/iron-binary-build-main.yml @@ -4,10 +4,6 @@ name: Iron Binary Build - main on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-binary-build-testing.yml b/.github/workflows/iron-binary-build-testing.yml index 25a693dc23..37e3524ccd 100644 --- a/.github/workflows/iron-binary-build-testing.yml +++ b/.github/workflows/iron-binary-build-testing.yml @@ -4,10 +4,6 @@ name: Iron Binary Build - testing on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml new file mode 100644 index 0000000000..58787a804c --- /dev/null +++ b/.github/workflows/iron-debian-build.yml @@ -0,0 +1,20 @@ +name: Debian Iron Source Build +on: + workflow_dispatch: + pull_request: + branches: + - iron + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + iron_debian: + name: Iron debian build + uses: ./.github/workflows/reusable-debian-build.yml + with: + ros_distro: iron + upstream_workspace: ros2_controllers.iron.repos + ref_for_scheduled_build: iron + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index 5664d61768..90dac67a44 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -1,7 +1,7 @@ -name: Iron RHEL Binary Build +name: RHEL Iron Binary Build on: workflow_dispatch: - push: + pull_request: branches: - iron schedule: @@ -12,17 +12,9 @@ on: jobs: iron_rhel_binary: name: Iron RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: iron - container: ghcr.io/ros-controls/ros:iron-rhel - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - - run: | - rosdep update - rosdep install -iy --from-path src/ros2_controllers - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test + uses: ./.github/workflows/reusable-rhel-binary-build.yml + with: + ros_distro: iron + upstream_workspace: ros2_controllers.iron.repos + ref_for_scheduled_build: iron + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/iron-semi-binary-build-main.yml b/.github/workflows/iron-semi-binary-build-main.yml index 2224a59f0e..ed90a46ea8 100644 --- a/.github/workflows/iron-semi-binary-build-main.yml +++ b/.github/workflows/iron-semi-binary-build-main.yml @@ -3,10 +3,6 @@ name: Iron Semi-Binary Build - main on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-semi-binary-build-testing.yml b/.github/workflows/iron-semi-binary-build-testing.yml index c5ff430c89..d06a20443d 100644 --- a/.github/workflows/iron-semi-binary-build-testing.yml +++ b/.github/workflows/iron-semi-binary-build-testing.yml @@ -3,10 +3,6 @@ name: Iron Semi-Binary Build - testing on: workflow_dispatch: - branches: - - iron - - '*feature*' - - '*feature/**' pull_request: branches: - iron diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml index 1e9d865c49..34372a4178 100644 --- a/.github/workflows/iron-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -1,8 +1,6 @@ name: Iron Source Build on: workflow_dispatch: - branches: - - iron push: branches: - iron diff --git a/.github/workflows/reusable-debian-build.yml b/.github/workflows/reusable-debian-build.yml new file mode 100644 index 0000000000..b406fe6eaa --- /dev/null +++ b/.github/workflows/reusable-debian-build.yml @@ -0,0 +1,63 @@ +name: Reusable Debian Source Build +# Reusable action to simplify dealing with debian source builds +# author: Christoph Froehlich + +on: + workflow_call: + inputs: + ros_distro: + description: 'ROS2 distribution name' + required: true + type: string + ref_for_scheduled_build: + description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' + default: '' + required: false + type: string + upstream_workspace: + description: 'Path to local .repos file.' + default: '' + required: false + type: string + skip_packages: + description: 'Packages to skip from build and test' + default: '' + required: false + type: string + + +jobs: + debian_source: + name: ${{ inputs.ros_distro }} debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: ${{ inputs.ros_distro }} + path: src/ros2_controllers + container: ghcr.io/ros-controls/ros:${{ inputs.ros_distro }}-debian + steps: + - name: Checkout default ref when build is not scheduled + if: ${{ github.event_name != 'schedule' }} + uses: actions/checkout@v4 + with: + path: ${{ env.path }} + - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build + if: ${{ github.event_name == 'schedule' }} + uses: actions/checkout@v4 + with: + ref: ${{ inputs.ref_for_scheduled_build }} + path: ${{ env.path }} + - name: Build workspace + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + if [[ -n "${{ inputs.upstream_workspace }}" ]]; then + vcs import src < ${{ env.path }}/${{ inputs.upstream_workspace }} + fi + colcon build --packages-up-to $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + - name: Test workspace + shell: bash + continue-on-error: true + run: | + source /opt/ros2_ws/install/setup.bash + colcon test --packages-select $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + colcon test-result --verbose diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index acefeebfac..234ec27677 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -56,10 +56,10 @@ jobs: BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.upstream_workspace }}-${{ inputs.ros_repo }}-${{ github.job }} steps: - - name: Checkout ${{ inputs.ref }} when build is not scheduled + - name: Checkout default ref when build is not scheduled if: ${{ github.event_name != 'schedule' }} uses: actions/checkout@v4 - - name: Checkout ${{ inputs.ref }} on scheduled build + - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build if: ${{ github.event_name == 'schedule' }} uses: actions/checkout@v4 with: diff --git a/.github/workflows/reusable-rhel-binary-build.yml b/.github/workflows/reusable-rhel-binary-build.yml new file mode 100644 index 0000000000..be4eabb76b --- /dev/null +++ b/.github/workflows/reusable-rhel-binary-build.yml @@ -0,0 +1,69 @@ +name: Reusable RHEL Binary Build +# Reusable action to simplify dealing with RHEL binary builds +# author: Christoph Froehlich + +on: + workflow_call: + inputs: + ros_distro: + description: 'ROS2 distribution name' + required: true + type: string + ref_for_scheduled_build: + description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' + default: '' + required: false + type: string + upstream_workspace: + description: 'Path to local .repos file.' + default: '' + required: false + type: string + skip_packages: + description: 'Packages to skip from build and test' + default: '' + required: false + type: string + +jobs: + rhel_binary: + name: ${{ inputs.ros_distro }} RHEL binary build + runs-on: ubuntu-latest + env: + path: src/ros2_controllers + container: ghcr.io/ros-controls/ros:${{ inputs.ros_distro }}-rhel + steps: + - name: Checkout default ref when build is not scheduled + if: ${{ github.event_name != 'schedule' }} + uses: actions/checkout@v4 + with: + path: ${{ env.path }} + - name: Checkout ${{ inputs.ref_for_scheduled_build }} on scheduled build + if: ${{ github.event_name == 'schedule' }} + uses: actions/checkout@v4 + with: + ref: ${{ inputs.ref_for_scheduled_build }} + path: ${{ env.path }} + - name: Install dependencies + run: | + source /opt/ros/${{ inputs.ros_distro }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + if [[ -n "${{ inputs.upstream_workspace }}" ]]; then + vcs import src < ${{ env.path }}/${{ inputs.upstream_workspace }} + fi + rosdep update + rosdep install -iyr --from-path src || true + - name: Build workspace + # source also underlay workspace with generate_parameter_library on rhel9 + run: | + source /opt/ros/${{ inputs.ros_distro }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + colcon build --packages-up-to $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + - name: Test workspace + shell: bash + continue-on-error: true + run: | + source /opt/ros/${{ inputs.ros_distro }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + colcon test --packages-select $(colcon list --paths ${{ env.path }}/* --names-only) --packages-skip ${{ inputs.skip_packages }} + colcon test-result --verbose diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index fcc1a297fd..3d5bc1cf35 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,7 +32,7 @@ jobs: - uses: actions/checkout@v4 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ inputs.ros_distro }} ref: ${{ inputs.ref }} @@ -63,7 +63,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v4.0.0 + - uses: actions/upload-artifact@v4.3.1 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log diff --git a/.github/workflows/rolling-binary-build-main.yml b/.github/workflows/rolling-binary-build-main.yml index 793db5d7e5..729d5e38ba 100644 --- a/.github/workflows/rolling-binary-build-main.yml +++ b/.github/workflows/rolling-binary-build-main.yml @@ -4,10 +4,6 @@ name: Rolling Binary Build - main on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master diff --git a/.github/workflows/rolling-binary-build-testing.yml b/.github/workflows/rolling-binary-build-testing.yml index 9b480d99c3..0596aeec56 100644 --- a/.github/workflows/rolling-binary-build-testing.yml +++ b/.github/workflows/rolling-binary-build-testing.yml @@ -4,10 +4,6 @@ name: Rolling Binary Build - testing on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml new file mode 100644 index 0000000000..153f4df681 --- /dev/null +++ b/.github/workflows/rolling-debian-build.yml @@ -0,0 +1,20 @@ +name: Debian Rolling Source Build +on: + workflow_dispatch: + pull_request: + branches: + - master + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + rolling_debian: + name: Rolling debian build + uses: ./.github/workflows/reusable-debian-build.yml + with: + ros_distro: rolling + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 04dc58775f..31c133ab69 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -1,7 +1,7 @@ -name: Rolling RHEL Binary Build +name: RHEL Rolling Binary Build on: workflow_dispatch: - push: + pull_request: branches: - master schedule: @@ -12,17 +12,9 @@ on: jobs: rolling_rhel_binary: name: Rolling RHEL binary build - runs-on: ubuntu-latest - env: - ROS_DISTRO: rolling - container: ghcr.io/ros-controls/ros:rolling-rhel - steps: - - uses: actions/checkout@v4 - with: - path: src/ros2_controllers - - run: | - rosdep update - rosdep install -iy --from-path src/ros2_controllers - source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build - colcon test + uses: ./.github/workflows/reusable-rhel-binary-build.yml + with: + ros_distro: rolling + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/rolling-semi-binary-build-main.yml b/.github/workflows/rolling-semi-binary-build-main.yml index 8b395e5163..206ca8bd52 100644 --- a/.github/workflows/rolling-semi-binary-build-main.yml +++ b/.github/workflows/rolling-semi-binary-build-main.yml @@ -3,10 +3,6 @@ name: Rolling Semi-Binary Build - main on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.github/workflows/rolling-semi-binary-build-testing.yml index 630881dc0a..b284c0b7d4 100644 --- a/.github/workflows/rolling-semi-binary-build-testing.yml +++ b/.github/workflows/rolling-semi-binary-build-testing.yml @@ -3,10 +3,6 @@ name: Rolling Semi-Binary Build - testing on: workflow_dispatch: - branches: - - master - - '*feature*' - - '*feature/**' pull_request: branches: - master diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index a314802327..3521258b1a 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 6c318219b2..fe22ca10b8 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.4.0 + 4.5.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface ros2_control_test_assets diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 480e90e166..ef5454a16c 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -46,41 +46,43 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); } -TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + state_if_conf.names[STATE_STEER_LEFT_WHEEL], controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index a2849d5742..a047186d14 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -62,7 +62,7 @@ class TestableAckermannSteeringController : public ackermann_steering_controller::AckermannSteeringController { FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(AckermannSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(AckermannSteeringControllerTest, activate_success); FRIEND_TEST(AckermannSteeringControllerTest, update_success); FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); @@ -147,7 +147,9 @@ class AckermannSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 2d951588c5..1a16bed838 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -48,41 +48,43 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); } -TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + state_if_conf.names[STATE_STEER_LEFT_WHEEL], controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 826a7aa6be..ba1a25c0ab 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index 0e4469cd50..8056a017d7 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -17,10 +17,14 @@ ROS 2 interface of the controller Parameters ^^^^^^^^^^^ -The admittance controller's uses the `generate_parameter_library `_ to handle its parameters. -The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -An example parameter file can be found in the `test folder of the controller `_ +The admittance controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +.. generate_parameter_library_details:: ../src/admittance_controller_parameters.yaml + +An example parameter file for this controller can be found in `the test folder `_: + +.. literalinclude:: ../test/test_params.yaml + :language: yaml Topics ^^^^^^^ diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index fd7cf32401..e690330aa0 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.4.0 + 4.5.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar @@ -34,6 +34,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing kinematics_interface_kdl ros2_control_test_assets diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index fe1d3214e0..6b03249df8 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -157,12 +157,15 @@ TEST_F(AdmittanceControllerTest, check_interfaces) auto command_interfaces = controller_->command_interface_configuration(); ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); auto state_interfaces = controller_->state_interface_configuration(); ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->state_interfaces_.size(), diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index a1f79c922b..44d8e5051d 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 2082124de6..8bb6ac79fa 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.4.0 + 4.5.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar @@ -28,6 +28,7 @@ ament_cmake_gmock controller_manager hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 06b0c7e846..3dcdc0b1db 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -44,29 +44,29 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); } -TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_WHEEL], - rear_wheels_names_[0] + "/" + traction_interface_name_); + cmd_if_conf.names[CMD_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_WHEEL], + state_if_conf.names[STATE_TRACTION_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 521506762b..5e21ff228c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -60,7 +60,7 @@ class TestableBicycleSteeringController : public bicycle_steering_controller::BicycleSteeringController { FRIEND_TEST(BicycleSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(BicycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(BicycleSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(BicycleSteeringControllerTest, activate_success); FRIEND_TEST(BicycleSteeringControllerTest, update_success); FRIEND_TEST(BicycleSteeringControllerTest, deactivate_success); @@ -144,7 +144,9 @@ class BicycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index 875910ba23..bc3a182753 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -46,31 +46,31 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); } -TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_WHEEL], + cmd_if_conf.names[CMD_TRACTION_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); - EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], + cmd_if_conf.names[CMD_STEER_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); - + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_WHEEL], + state_if_conf.names[STATE_TRACTION_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 316cd9e52d..4a517f8a6d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* [diff_drive] Remove unused parameter and add simple validation #abi-breaking (`#958 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 436832c523..d67815b5e0 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -53,8 +53,7 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_diff_drive_controller - test/test_diff_drive_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) + test/test_diff_drive_controller.cpp) target_link_libraries(test_diff_drive_controller diff_drive_controller ) @@ -69,8 +68,9 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock(test_load_diff_drive_controller + add_rostest_with_parameters_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml ) ament_target_dependencies(test_load_diff_drive_controller controller_manager diff --git a/diff_drive_controller/doc/parameters_context.yaml b/diff_drive_controller/doc/parameters_context.yaml new file mode 100644 index 0000000000..81e92806f5 --- /dev/null +++ b/diff_drive_controller/doc/parameters_context.yaml @@ -0,0 +1,9 @@ +linear.x: | + Joint limits structure for the linear ``x``-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. + +angular.z: | + Joint limits structure for the rotation about ``z``-axis. + The limiter ignores position limits. + For details see ``joint_limits`` package from ros2_control repository. diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index d2dd284cf3..70d0d7ca5c 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -64,17 +64,12 @@ Publishers Parameters ,,,,,,,,,,,, -Check `parameter definition file for details `_. +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -Note that the documentation on parameters for joint limits can be found in `their header file `_. -Those parameters are: +.. generate_parameter_library_details:: ../src/diff_drive_controller_parameter.yaml + parameters_context.yaml -linear.x [JointLimits structure] - Joint limits structure for the linear X-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. +An example parameter file for this controller can be found in `the test directory `_: -angular.z [JointLimits structure] - Joint limits structure for the rotation about Z-axis. - The limiter ignores position limits. - For details see ``joint_limits`` package from ros2_control repository. +.. literalinclude:: ../test/config/test_diff_drive_controller.yaml + :language: yaml diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 554bedba59..72b38f7d2d 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 @@ -110,6 +110,12 @@ class DiffDriveController : public controller_interface::ControllerInterface // Parameters from ROS for diff_drive_controller std::shared_ptr param_listener_; Params params_; + /* Number of wheels on each side of the robot. This is important to take the wheels slip into + * account when multiple wheels on each side are present. If there are more wheels then control + * signals for each side, you should enter number for control signals. For example, Husky has two + * wheels on each side, but they use one control signal, in this case '1' is the correct value of + * the parameter. */ + int wheels_per_side_; Odometry odometry_; diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 9f79dabea6..e87ab85471 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.4.0 + 4.5.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios @@ -26,6 +26,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index ea08aef89b..42b6cda8e1 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -149,7 +149,7 @@ controller_interface::return_type DiffDriveController::update( { double left_feedback_mean = 0.0; double right_feedback_mean = 0.0; - for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -166,8 +166,8 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= static_cast(params_.wheels_per_side); - right_feedback_mean /= static_cast(params_.wheels_per_side); + left_feedback_mean /= static_cast(wheels_per_side_); + right_feedback_mean /= static_cast(wheels_per_side_); if (params_.position_feedback) { @@ -257,7 +257,7 @@ controller_interface::return_type DiffDriveController::update( (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); @@ -286,12 +286,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (params_.left_wheel_names.empty()) - { - RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); - return controller_interface::CallbackReturn::ERROR; - } - const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; @@ -320,7 +314,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } // left and right sides are both equal at this point - params_.wheels_per_side = params_.left_wheel_names.size(); + wheels_per_side_ = static_cast(params_.left_wheel_names.size()); if (publish_limited_velocity_) { diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 0c0285e7c2..9720e068e1 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -2,23 +2,24 @@ diff_drive_controller: left_wheel_names: { type: string_array, default_value: [], - description: "Link names of the left side wheels", + description: "Names of the left side wheels' joints", + validation: { + not_empty<>: [] + } } right_wheel_names: { type: string_array, default_value: [], - description: "Link names of the right side wheels", + description: "Names of the right side wheels' joints", + validation: { + not_empty<>: [] + } } wheel_separation: { type: double, default_value: 0.0, description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", } - wheels_per_side: { - type: int, - default_value: 0, - description: "Number of wheels on each side of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number for control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", - } wheel_radius: { type: double, default_value: 0.0, diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index a2149eb6bc..bfbf8f2d19 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -2,7 +2,6 @@ test_diff_drive_controller: ros__parameters: left_wheel_names: ["left_wheels"] right_wheel_names: ["right_wheels"] - write_op_modes: ["motor_controller"] wheel_separation: 0.40 wheels_per_side: 1 # actually 2, but both are controlled by 1 signal @@ -21,7 +20,7 @@ test_diff_drive_controller: open_loop: true enable_odom_tf: true - cmd_vel_timeout: 500 # milliseconds + cmd_vel_timeout: 0.5 # seconds publish_limited_velocity: true velocity_rolling_window_size: 10 diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index eb970d34a3..9ab3022a9f 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -36,6 +36,12 @@ using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; using testing::SizeIs; +namespace +{ +const std::vector left_wheel_names = {"left_wheel_joint"}; +const std::vector right_wheel_names = {"right_wheel_joint"}; +} // namespace + class TestableDiffDriveController : public diff_drive_controller::DiffDriveController { public: @@ -166,11 +172,28 @@ class TestDiffDriveController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + controller_interface::return_type InitController( + const std::vector left_wheel_joints_init = left_wheel_names, + const std::vector right_wheel_joints_init = right_wheel_names, + const std::vector & parameters = {}, const std::string ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); + parameter_overrides.push_back( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); + + 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); + } + const std::string controller_name = "test_diff_drive_controller"; std::unique_ptr controller_; - const std::vector left_wheel_names = {"left_wheel_joint"}; - const std::vector right_wheel_names = {"right_wheel_joint"}; std::vector position_values_ = {0.1, 0.2}; std::vector velocity_values_ = {0.01, 0.02}; @@ -193,92 +216,57 @@ class TestDiffDriveController : public ::testing::Test const std::string urdf_ = ""; }; -TEST_F(TestDiffDriveController, configure_fails_without_parameters) +TEST_F(TestDiffDriveController, init_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + const auto ret = + controller_->init(controller_name, urdf_, 0, "", controller_->define_custom_node_options()); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); } -TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined) +TEST_F(TestDiffDriveController, init_fails_with_only_left_or_only_right_side_defined) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(InitController(left_wheel_names, {}), controller_interface::return_type::ERROR); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(std::vector()))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(std::vector()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(InitController({}, right_wheel_names), controller_interface::return_type::ERROR); } TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - - auto extended_right_wheel_names = right_wheel_names; - extended_right_wheel_names.push_back("extra_wheel"); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); + ASSERT_EQ( + InitController(left_wheel_names, {right_wheel_names[0], "extra_wheel"}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_THAT( - controller_->state_interface_configuration().names, - SizeIs(left_wheel_names.size() + right_wheel_names.size())); - ASSERT_THAT( - controller_->command_interface_configuration().names, - SizeIs(left_wheel_names.size() + right_wheel_names.size())); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -292,26 +280,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -327,26 +307,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -363,26 +335,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -398,26 +363,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -435,26 +393,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, urdf_, 0, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -469,13 +420,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -483,15 +428,9 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); // We implicitly test that by default position feedback is required - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -499,15 +438,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -516,15 +451,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); @@ -533,15 +464,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -550,15 +477,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 0.1)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -599,15 +522,11 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 1eb8939031..4c9d2f984f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -16,13 +16,14 @@ #include #include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadDiffDriveController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -33,6 +34,14 @@ TEST(TestLoadDiffDriveController, load_controller) ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); rclcpp::shutdown(); + return result; } diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 501c231def..1a9ffce714 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -42,7 +42,9 @@ The following is a step-by-step guide to create source files, basic tests, and c 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``on_init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers `_. - 6. (optional) Often, controllers accept lists of joint names and interface names as parameters. + 6. (Optional) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``get_node_options``. + + 7. (Optional) Often, controllers accept lists of joint names and interface names as parameters. If so, you can add two protected string vectors to store those values. 4. **Adding definitions into source file (.cpp)** diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 54cdc18d69..2cb40f6e36 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ + 4.4.0 (2024-01-11) ------------------ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index ee5504c672..279d5fbf43 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.4.0 + 4.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios @@ -17,6 +17,8 @@ ament_cmake_gmock controller_manager + hardware_interface_testing + hardware_interface ros2_control_test_assets diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index f9d72ab202..200a1beda8 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -54,7 +54,8 @@ void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); } void JointGroupEffortControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_effort_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_effort_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index 61bb1ddf9a..52f1f9934a 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupVelocityController, load_controller) diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 58191c2f42..fc05235ade 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Revert "[ForceTorqueSensorBroadcaster] Create ParamListener and get parameters on configure (`#698 `_)" (`#988 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.4.0 (2024-01-11) ------------------ diff --git a/force_torque_sensor_broadcaster/doc/parameters_context.yaml b/force_torque_sensor_broadcaster/doc/parameters_context.yaml new file mode 100644 index 0000000000..6991427316 --- /dev/null +++ b/force_torque_sensor_broadcaster/doc/parameters_context.yaml @@ -0,0 +1,12 @@ +interface_names: | + (optional) Defines custom, per axis interface names. + This is used if different prefixes, i.e., sensor name, or non-standard interface names are used. + It is sufficient that only one ``interface_name`` is defined. + This enables the broadcaster to use force sensing cells with less than six measuring axes. + An example definition is: + + .. code-block:: yaml + + interface_names: + force: + x: example_name/example_interface diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index 053723e8f0..df0430e3bb 100644 --- a/force_torque_sensor_broadcaster/doc/userdoc.rst +++ b/force_torque_sensor_broadcaster/doc/userdoc.rst @@ -12,25 +12,17 @@ The controller is a wrapper around ``ForceTorqueSensor`` semantic component (see Parameters ^^^^^^^^^^^ -The interfaces can be defined in two ways, using ``sensor_name`` or ``interface_names`` parameter. -Those two parameters can not be defined at the same time +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -frame_id (mandatory) - Frame in which the output message will be published. +The interfaces can be defined in two ways, using the ``sensor_name`` or the ``interface_names`` parameter: +Those two parameters cannot be defined at the same time. -sensor_name (optional) - Defines sensor name used as prefix for its interfaces. - If used standard interface names for a 6D FTS will be used: /force.x, ..., /torque.z. +Full list of parameters: -interface_names.[force|torque].[x|y|z] (optional) - Defines custom, per axis interface names. - This is used if different prefixes, i.e., sensor name, or non-standard interface names are used. - It is sufficient that only one ``interface_name`` is defined. - This enables broadcaster use for force sensing cells with less then six measuring axes. - Example definition is: +.. generate_parameter_library_details:: ../src/force_torque_sensor_broadcaster_parameters.yaml + parameters_context.yaml - .. code-block:: yaml +An example parameter file for this controller can be found in `the test directory `_: - interface_names: - force: - x: example_name/example_interface +.. literalinclude:: ../test/force_torque_sensor_broadcaster_params.yaml + :language: yaml diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index be9fc21d16..0791eb5d16 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.4.0 + 4.5.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl @@ -23,6 +23,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index e9a173380b..9b570d353f 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -29,12 +29,6 @@ ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() } controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init() -{ - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { try { @@ -43,10 +37,18 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( } catch (const std::exception & e) { - fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what()); + 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 ForceTorqueSensorBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + const bool no_interface_names_defined = params_.interface_names.force.x.empty() && params_.interface_names.force.y.empty() && params_.interface_names.force.z.empty() && params_.interface_names.torque.x.empty() && 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 68a85d9d8e..3e75ab6012 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -10,7 +10,8 @@ force_torque_sensor_broadcaster: sensor_name: { type: string, default_value: "", - description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined. + If used, standard interface names for a 6D FTS will be used: ``/force.x, ..., /torque.z``", } interface_names: force: diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index ce6a04ec1f..2412361352 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 @@ -55,7 +55,9 @@ void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullp void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() { - const auto result = fts_broadcaster_->init("test_force_torque_sensor_broadcaster", "", 0); + const auto result = fts_broadcaster_->init( + "test_force_torque_sensor_broadcaster", "", 0, "", + fts_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; @@ -113,12 +115,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'interface_names' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // configure failed, both 'sensor_name' and 'interface_names' supplied ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -129,7 +130,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSe SetUpFTSBroadcaster(); // set the 'sensor_name' empty - fts_broadcaster_->get_node()->declare_parameter("sensor_name", ""); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", ""}); // configure failed, 'sensor_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -140,8 +141,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSe SetUpFTSBroadcaster(); // set the 'interface_names' empty - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", ""); - fts_broadcaster_->get_node()->declare_parameter("interface_names.torque.z", ""); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", ""}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", ""}); // configure failed, 'interface_name' parameter empty ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); @@ -152,10 +153,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) SetUpFTSBroadcaster(); // set the 'sensor_name' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); // set the 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -163,8 +164,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) // check interface configuration auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = fts_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) @@ -172,12 +175,11 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) SetUpFTSBroadcaster(); // set the 'interface_names' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); // set the 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure passed ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -188,8 +190,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); // configure and activate success ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -198,8 +200,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_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); @@ -207,8 +211,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) // check interface configuration cmd_if_conf = fts_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = fts_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); // did not change + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) @@ -216,8 +222,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -232,10 +238,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -249,8 +254,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) SetUpFTSBroadcaster(); // set the params 'sensor_name' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("sensor_name", sensor_name_); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -272,10 +277,9 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -297,16 +301,13 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) SetUpFTSBroadcaster(); // set all the params 'interface_names' and 'frame_id' - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.x", "fts_sensor/force.x"); - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.y", "fts_sensor/force.y"); - fts_broadcaster_->get_node()->declare_parameter("interface_names.force.z", "fts_sensor/force.z"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.x", "fts_sensor/torque.x"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.y", "fts_sensor/torque.y"); - fts_broadcaster_->get_node()->declare_parameter( - "interface_names.torque.z", "fts_sensor/torque.z"); - fts_broadcaster_->get_node()->declare_parameter("frame_id", frame_id_); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.y", "fts_sensor/force.y"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.force.z", "fts_sensor/force.z"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.x", "fts_sensor/torque.x"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.y", "fts_sensor/torque.y"}); + fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index e6f5e3d52f..d0e32e8793 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index c0827e9e20..8950a9a3e9 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.4.0 + 4.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios @@ -22,6 +22,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 236cb14edd..c31c8a964c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -62,7 +62,8 @@ void ForwardCommandControllerTest::TearDown() { controller_.reset(nullptr); } void ForwardCommandControllerTest::SetUpController() { - const auto result = controller_->init("forward_command_controller", "", 0); + const auto result = controller_->init( + "forward_command_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; @@ -137,6 +138,7 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); } TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -189,6 +191,7 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); ASSERT_EQ( controller_->on_activate(rclcpp_lifecycle::State()), @@ -197,8 +200,10 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); } TEST_F(ForwardCommandControllerTest, CommandSuccessTest) @@ -352,6 +357,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); @@ -359,8 +365,10 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); auto command_msg = std::make_shared(); command_msg->data = {10.0, 20.0, 30.0}; @@ -383,8 +391,10 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); // did not change + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( 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 2d3b61e211..7879d5c1d7 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 @@ -64,7 +64,9 @@ void MultiInterfaceForwardCommandControllerTest::TearDown() { controller_.reset( void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params_and_activate) { - const auto result = controller_->init("multi_interface_forward_command_controller", "", 0); + const auto result = controller_->init( + "multi_interface_forward_command_controller", "", 0, "", + controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; @@ -297,6 +299,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // send command auto command_ptr = std::make_shared(); @@ -322,6 +325,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 677d52f784..9013897769 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/gripper_controllers/doc/userdoc.rst b/gripper_controllers/doc/userdoc.rst index 28262e90f9..7f51c8f4ac 100644 --- a/gripper_controllers/doc/userdoc.rst +++ b/gripper_controllers/doc/userdoc.rst @@ -5,10 +5,23 @@ Gripper Action Controller -------------------------------- -Controller for executing a gripper command action for simple single-dof grippers. +Controllers for executing a gripper command action for simple single-dof grippers: + +- ``position_controllers/GripperActionController`` +- ``effort_controllers/GripperActionController`` Parameters ^^^^^^^^^^^ -This controller uses the `generate_parameter_library `_ to handle its parameters. +These controllers use the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= .. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml + + +An example parameter file +========================= + +.. generate_parameter_library_default:: + ../src/gripper_action_controller_parameters.yaml diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 9488804f1d..a35fce7894 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.4.0 + 4.5.0 The gripper_controllers package Bence Magyar @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index da9e15840e..9f7e024917 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -62,7 +62,8 @@ void GripperControllerTest::TearDown() template void GripperControllerTest::SetUpController() { - const auto result = controller_->init("gripper_controller", "", 0); + const auto result = + controller_->init("gripper_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 130b12e0bb..0ef5f0bcb2 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadGripperActionControllers, load_controller) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 821ef8b385..61d843c659 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/imu_sensor_broadcaster/doc/userdoc.rst b/imu_sensor_broadcaster/doc/userdoc.rst index 3b8ae172db..09b51a7ecb 100644 --- a/imu_sensor_broadcaster/doc/userdoc.rst +++ b/imu_sensor_broadcaster/doc/userdoc.rst @@ -11,6 +11,17 @@ The controller is a wrapper around ``IMUSensor`` semantic component (see ``contr Parameters ^^^^^^^^^^^ -This controller uses the `generate_parameter_library `_ to handle its parameters. +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +List of parameters +========================= .. generate_parameter_library_details:: ../src/imu_sensor_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/imu_sensor_broadcaster_params.yaml + :language: yaml diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 184f9f7dd7..5694e1cee7 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.4.0 + 4.5.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl @@ -25,6 +25,7 @@ ament_lint_auto ament_lint_common controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 83edc044d3..25a39a8b4d 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -55,7 +55,8 @@ void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() { - const auto result = imu_broadcaster_->init("test_imu_sensor_broadcaster", "", 0); + const auto result = imu_broadcaster_->init( + "test_imu_sensor_broadcaster", "", 0, "", imu_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector state_ifs; @@ -119,8 +120,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) // check interface configuration auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) @@ -138,8 +141,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // check interface configuration auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // deactivate passed ASSERT_EQ(imu_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -147,8 +152,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // check interface configuration cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); // did not change + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 0650f4e762..cd80339155 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml new file mode 100644 index 0000000000..c96f8301ae --- /dev/null +++ b/joint_state_broadcaster/doc/joint_state_broadcaster_parameter_context.yml @@ -0,0 +1,46 @@ +map_interface_to_joint_state: | + Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message. + Usecases: + + #. Hydraulics robots where feedback and commanded values often have an offset and reliance on open-loop control is common. + Typically one would map both values in separate interfaces in the framework. + To visualize those data multiple joint_state_broadcaster instances and robot_state_publishers would be used to visualize both values in RViz. + #. A robot provides multiple measuring techniques for its joint values which results in slightly different values. + Typically one would use separate interface for providing those values in the framework. + Using multiple joint_state_broadcaster instances we could publish and show both in RViz. + + Format (each line is optional): + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tposition: + \t\tvelocity: + \t\teffort: + + + Examples: + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tposition: kf_estimated_position + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\tvelocity: derived_velocity + \t\teffort: derived_effort + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\teffort: torque_sensor + + + .. code-block:: yaml + + \tmap_interface_to_joint_state: + \t\teffort: current_sensor diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index c9164ec723..c7bf4fa9a1 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -22,70 +22,19 @@ If none of the requested interface are not defined, the controller returns error 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. -use_local_topics - Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``. +List of parameters +,,,,,,,,,,,,,,,,,, -joints - Optional parameter (string array) to support broadcasting of only specific joints and interfaces. - It has to be used in combination with the ``interfaces`` parameter. - Joint state broadcaster asks for access to all defined interfaces on all defined joints. +.. generate_parameter_library_details:: + ../src/joint_state_broadcaster_parameters.yaml + joint_state_broadcaster_parameter_context.yml -interfaces - Optional parameter (string array) to support broadcasting of only specific joints and interfaces. - It has to be used in combination with the ``joints`` parameter. +An example parameter file +,,,,,,,,,,,,,,,,,,,,,,,,, - -extra_joints - Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0. - - -map_interface_to_joint_state - Optional parameter (map) providing mapping between custom interface names to standard fields in ``joint_states`` message. - Usecases: - - 1. Hydraulics robots where feedback and commanded values often have an offset and reliance on open-loop control is common. - Typically one would map both values in separate interfaces in the framework. - To visualize those data multiple joint_state_broadcaster instances and robot_state_publishers would be used to visualize both values in RViz. - - 1. A robot provides multiple measuring techniques for its joint values which results in slightly different values. - Typically one would use separate interface for providing those values in the framework. - Using multiple joint_state_broadcaster instances we could publish and show both in RViz. - - Format (each line is optional): - - .. code-block:: yaml - - map_interface_to_joint_state: - position: - velocity: - effort: - - - Examples: - - .. code-block:: yaml - - map_interface_to_joint_state: - position: kf_estimated_position - - - .. code-block:: yaml - - map_interface_to_joint_state: - velocity: derived_velocity - effort: derived_effort - - - .. code-block:: yaml - - map_interface_to_joint_state: - effort: torque_sensor - - - .. code-block:: yaml - - map_interface_to_joint_state: - effort: current_sensor +.. generate_parameter_library_default:: + ../src/joint_state_broadcaster_parameters.yaml diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 70d0f7daca..6dd8b4b61e 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.4.0 + 4.5.0 Broadcaster to publish joint state Bence Magyar Denis Stogl @@ -25,6 +25,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface rclcpp ros2_control_test_assets diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index ba0d4f0051..8f0d4da6c5 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -2,18 +2,29 @@ joint_state_broadcaster: use_local_topics: { type: bool, default_value: false, + description: "Defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``." } joints: { type: string_array, default_value: [], + description: "Parameter to support broadcasting of only specific joints and interfaces. + It has to be used in combination with the ``interfaces`` parameter. + If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be + published. + Joint state broadcaster asks for access to all defined interfaces on all defined joints." } extra_joints: { type: string_array, default_value: [], + description: "Names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0." } interfaces: { type: string_array, default_value: [], + description: "Parameter to support broadcasting of only specific joints and interfaces. + It has to be used in combination with the ``joints`` parameter. + If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be + published." } map_interface_to_joint_state: position: { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 3e45740bbc..2faa55f467 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -69,7 +69,8 @@ void JointStateBroadcasterTest::SetUpStateBroadcaster( void JointStateBroadcasterTest::init_broadcaster_and_set_parameters( const std::vector & joint_names, const std::vector & interfaces) { - const auto result = state_broadcaster_->init("joint_state_broadcaster", "", 0); + const auto result = state_broadcaster_->init( + "joint_state_broadcaster", "", 0, "", state_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); state_broadcaster_->get_node()->set_parameter({"joints", joint_names}); @@ -180,8 +181,10 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -227,8 +230,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -274,8 +279,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -321,8 +328,10 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -360,9 +369,11 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) // check interface configuration cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT( state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); // does not change + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) @@ -381,8 +392,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -455,8 +468,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -504,8 +519,10 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; @@ -547,8 +564,10 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 5eb851589c..a41c70cbc7 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* [JTC] Fill action error_strings (`#887 `_) +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* [JTC] Invalidate empty trajectory messages (`#902 `_) +* Revert "[JTC] Remove read_only from 'joints', 'state_interfaces' and 'command_interfaces' parameters (`#967 `_)" (`#978 `_) +* [JTC] Convert lambda to class functions (`#945 `_) +* Contributors: Christoph Fröhlich, Noel Jiménez García + 4.4.0 (2024-01-11) ------------------ * Cancel goal in on_deactivate (`#962 `_) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 5cddb1bf4d..1006e6bc7a 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -12,6 +12,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS controller_interface generate_parameter_library hardware_interface + joint_limits + nav_msgs pluginlib rclcpp rclcpp_lifecycle @@ -19,6 +21,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rsl tl_expected trajectory_msgs + tf2 + tf2_geometry_msgs ) find_package(ament_cmake REQUIRED) @@ -49,6 +53,20 @@ ament_target_dependencies(joint_trajectory_controller PUBLIC ${THIS_PACKAGE_INCL # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(joint_trajectory_controller PRIVATE "JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL" "_USE_MATH_DEFINES") + +add_library(cartesian_trajectory_generator SHARED + src/cartesian_trajectory_generator.cpp +) +target_compile_features(cartesian_trajectory_generator PUBLIC cxx_std_17) +target_include_directories(cartesian_trajectory_generator PUBLIC + $ + $ +) +target_link_libraries(cartesian_trajectory_generator PUBLIC + joint_trajectory_controller +) +ament_target_dependencies(cartesian_trajectory_generator PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + pluginlib_export_plugin_description_file(controller_interface joint_trajectory_plugin.xml) if(BUILD_TESTING) @@ -86,6 +104,20 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_actions joint_trajectory_controller ) + + ament_add_gmock(test_load_cartesian_trajectory_generator + test/test_load_cartesian_trajectory_generator.cpp + ) + target_link_libraries(test_load_cartesian_trajectory_generator + cartesian_trajectory_generator + ) + ament_target_dependencies(test_load_cartesian_trajectory_generator + controller_manager + control_toolbox + realtime_tools + ros2_control_test_assets + ) + endif() @@ -94,6 +126,7 @@ install( DESTINATION include/joint_trajectory_controller ) install(TARGETS + cartesian_trajectory_generator joint_trajectory_controller joint_trajectory_controller_parameters EXPORT export_joint_trajectory_controller diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index 8ad2b406d6..dbb50fcbeb 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -5,146 +5,18 @@ Details about parameters ^^^^^^^^^^^^^^^^^^^^^^^^ -joints (list(string)) - Joint names to control and listen to. +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. -command_joints (list(string)) - Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names. -command_interface (list(string)) - Command interfaces provided by the hardware interface for all joints. +List of parameters +========================= - Values: [position | velocity | acceleration] (multiple allowed) +.. generate_parameter_library_details:: + ../src/joint_trajectory_controller_parameters.yaml + parameters_context.yaml -state_interfaces (list(string)) - State interfaces provided by the hardware for all joints. +An example parameter file +========================= - Values: position (mandatory) [velocity, [acceleration]]. - Acceleration interface can only be used in combination with position and velocity. - -action_monitor_rate (double) - Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory). - - Default: 20.0 - -allow_partial_joints_goal (boolean) - Allow joint goals defining trajectory for only some joints. - - Default: false - -allow_integration_in_goal_trajectories (boolean) - Allow integration in goal trajectories to accept goals without position or velocity specified - - Default: false - -interpolation_method (string) - The type of interpolation to use, if any. Can be "splines" or "none". - - Default: splines - -open_loop_control (boolean) - Use controller in open-loop control mode: - - * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. - * It deactivates the feedback control, see the ``gains`` structure. - - This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). - - .. Note:: - If this flag is set, the controller tries to read the values from the command interfaces on activation. - If they have real numeric values, those will be used instead of state interfaces. - Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started. - - Default: false - -start_with_holding (bool) - If true, start with holding position after activation. Otherwise, no command will be sent until - the first trajectory is received. - - Default: true - -allow_nonzero_velocity_at_trajectory_end (boolean) - If false, the last velocity point has to be zero or the goal will be rejected. - - Default: false - -cmd_timeout (double) - Timeout after which the input command is considered stale. - Timeout is counted from the end of the trajectory (the last point). - ``cmd_timeout`` must be greater than ``constraints.goal_time``, - otherwise ignored. - - If zero, timeout is deactivated" - - Default: 0.0 - -constraints (structure) - Default values for tolerances if no explicit values are states in JointTrajectory message. - -constraints.stopped_velocity_tolerance (double) - Default value for end velocity deviation. - - Default: 0.01 - -constraints.goal_time (double) - Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time. - If set to zero, the controller will wait a potentially infinite amount of time. - - Default: 0.0 (not checked) - -constraints..trajectory (double) - Maximally allowed deviation from the target trajectory for a given joint. - - Default: 0.0 (tolerance is not enforced) - -constraints..goal (double) - Maximally allowed deviation from the goal (end of the trajectory) for a given joint. - - Default: 0.0 (tolerance is not enforced) - -gains (structure) - Only relevant, if ``open_loop_control`` is not set. - - If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. - This structure contains the controller gains for every joint with the control law - - .. math:: - - u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) - - with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below), - the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. - -gains..p (double) - Proportional gain :math:`k_p` for PID - - Default: 0.0 - -gains..i (double) - Integral gain :math:`k_i` for PID - - Default: 0.0 - -gains..d (double) - Derivative gain :math:`k_d` for PID - - Default: 0.0 - -gains..i_clamp (double) - Integral clamp. Symmetrical in both positive and negative direction. - - Default: 0.0 - -gains..ff_velocity_scale (double) - Feed-forward scaling :math:`k_{ff}` of velocity - - Default: 0.0 - -gains..angle_wraparound (bool) - For joints that wrap around (without end stop, ie. are continuous), - where the shortest rotation to the target position is the desired motion. - If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. - Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured - position :math:`s` from the state interface. - - Default: false +.. generate_parameter_library_default:: + ../src/joint_trajectory_controller_parameters.yaml diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml new file mode 100644 index 0000000000..2ffe8aed36 --- /dev/null +++ b/joint_trajectory_controller/doc/parameters_context.yaml @@ -0,0 +1,13 @@ +constraints: + Default values for tolerances if no explicit values are set in the ``JointTrajectory`` message. + +gains: | + Only relevant, if ``open_loop_control`` is not set. + + If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint. + This structure contains the controller gains for every joint with the control law + + .. math:: u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) + + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below), + the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp new file mode 100644 index 0000000000..4ff40c3730 --- /dev/null +++ b/joint_trajectory_controller/include/joint_trajectory_controller/cartesian_trajectory_generator.hpp @@ -0,0 +1,92 @@ +// Copyright (c) 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef JOINT_TRAJECTORY_CONTROLLER__CARTESIAN_TRAJECTORY_GENERATOR_HPP_ +#define JOINT_TRAJECTORY_CONTROLLER__CARTESIAN_TRAJECTORY_GENERATOR_HPP_ + +#include +#include +#include +#include + +#include "control_msgs/msg/cartesian_trajectory_generator_state.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "trajectory_msgs/msg/multi_dof_joint_trajectory_point.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace cartesian_trajectory_generator +{ +class CartesianTrajectoryGenerator : public joint_trajectory_controller::JointTrajectoryController +{ +public: + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + CartesianTrajectoryGenerator(); + + /** + * @brief command_interface_configuration This controller requires the position and velocity + * state interfaces for the controlled joints + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + using ControllerReferenceMsg = trajectory_msgs::msg::MultiDOFJointTrajectoryPoint; + using ControllerFeedbackMsg = nav_msgs::msg::Odometry; + +protected: + bool read_state_from_state_interfaces(JointTrajectoryPoint & state) override; + + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void publish_state( + const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) override; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_reliable_ = nullptr; + realtime_tools::RealtimeBuffer> reference_world_; + + rclcpp::Subscription::SharedPtr feedback_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> feedback_; + + trajectory_msgs::msg::JointTrajectoryPoint control_output_local_; + +private: + void reference_callback(const std::shared_ptr msg); + + using CartControllerStateMsg = control_msgs::msg::CartesianTrajectoryGeneratorState; + using CartStatePublisher = realtime_tools::RealtimePublisher; + using CartStatePublisherPtr = std::unique_ptr; + rclcpp::Publisher::SharedPtr cart_publisher_; + CartStatePublisherPtr cart_state_publisher_; + + // storage of last received measured position to + geometry_msgs::msg::Pose last_received_measured_position_; +}; + +} // namespace cartesian_trajectory_generator + +#endif // JOINT_TRAJECTORY_CONTROLLER__CARTESIAN_TRAJECTORY_GENERATOR_HPP_ 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 0ed2ce5688..130dbfbc3a 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 @@ -24,13 +24,17 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/srv/query_trajectory_state.hpp" +#include "control_msgs/srv/reset_dofs.hpp" #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "joint_trajectory_controller/visibility_control.h" +#include "pluginlib/class_loader.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" @@ -127,7 +131,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::shared_ptr param_listener_; Params params_; + // variables for storing internal data for open-loop control trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + rclcpp::Time last_commanded_time_; + /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ interpolation_methods::DEFAULT_INTERPOLATION}; @@ -136,6 +143,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // For convenience, for each type the interfaces are ordered so that i-th position // matches i-th index in joint_names_ template + using JointInterfaceRefs = std::vector>; + template using InterfaceReferences = std::vector>>; InterfaceReferences joint_command_interface_; @@ -164,6 +173,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa double cmd_timeout_; // True if holding position or repeating last trajectory point in case of success realtime_tools::RealtimeBuffer rt_is_holding_; + + // joint limiter configuration for JTC + using JointLimiter = joint_limits::JointLimiterInterface; + std::shared_ptr> joint_limiter_loader_; + std::unique_ptr joint_limiter_; + // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = @@ -171,9 +186,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Service::SharedPtr query_state_srv_; - std::shared_ptr traj_external_point_ptr_ = nullptr; + std::shared_ptr current_trajectory_ = nullptr; realtime_tools::RealtimeBuffer> - traj_msg_external_point_ptr_; + new_trajectory_msg_; std::shared_ptr hold_position_msg_ptr_ = nullptr; @@ -194,6 +209,18 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + using ControllerResetDofsSrvType = control_msgs::srv::ResetDofs; + + struct ResetDofsData + { + bool reset; + double position; + double velocity; + double acceleration; + }; + realtime_tools::RealtimeBuffer> reset_dofs_flags_; + rclcpp::Service::SharedPtr reset_dofs_service_; + // callback for topic interface JOINT_TRAJECTORY_CONTROLLER_PUBLIC void topic_callback(const std::shared_ptr msg); @@ -209,6 +236,26 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void goal_accepted_callback( std::shared_ptr> goal_handle); + // Callback for services + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void reset_dofs_service_callback( + const std::shared_ptr request, + std::shared_ptr response); + + using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; + + /** + * Computes the error for a specific joint in the trajectory. + * + * @param[out] error The computed error for the joint. + * @param[in] index The index of the joint in the trajectory. + * @param[in] current The current state of the joints. + * @param[in] desired The desired state of the joints. + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + void compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) const; // fill trajectory_msg so it matches joints controlled by this controller // positions set to current position, velocities, accelerations and efforts to 0.0 JOINT_TRAJECTORY_CONTROLLER_PUBLIC @@ -217,11 +264,11 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // sorts the joints of the incoming message to our local order JOINT_TRAJECTORY_CONTROLLER_PUBLIC void sort_to_local_joint_order( - std::shared_ptr trajectory_msg); + std::shared_ptr trajectory_msg) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void add_new_trajectory_msg( + virtual void add_new_trajectory_msg( const std::shared_ptr & traj_msg); JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool validate_trajectory_point_field( @@ -251,13 +298,13 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool has_active_trajectory() const; - using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint; JOINT_TRAJECTORY_CONTROLLER_PUBLIC - void publish_state( + virtual void publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); - void read_state_from_state_interfaces(JointTrajectoryPoint & state); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + virtual bool read_state_from_state_interfaces(JointTrajectoryPoint & state); /** Assign values from the command interfaces as state. * This is only possible if command AND state interfaces exist for the same type, @@ -272,6 +319,31 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const std::shared_ptr request, std::shared_ptr response); + // BEGIN: helper methods + template + void assign_point_from_interface( + std::vector & trajectory_point_interface, const JointInterfaceRefs & joint_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + trajectory_point_interface[index] = joint_interface[index].get().get_value(); + } + }; + + template + void assign_interface_from_point( + JointInterfaceRefs & joint_interface, const std::vector & trajectory_point_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + joint_interface[index].get().set_value(trajectory_point_interface[index]); + } + }; + + void resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + // END: helper methods + private: void update_pids(); @@ -279,10 +351,26 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const std::vector & interface_type_list, const std::string & interface_type); void init_hold_position_msg(); - void resize_joint_trajectory_point( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + + /** + * @brief Assigns the values from a trajectory point interface to a joint interface. + * + * @tparam T The type of the joint interface. + * @param[out] joint_interface The reference_wrapper to assign the values to + * @param[in] trajectory_point_interface Containing the values to assign. + * @todo: Use auto in parameter declaration with c++20 + */ + template + void assign_interface_from_point( + const T & joint_interface, const std::vector & trajectory_point_interface) + { + for (size_t index = 0; index < dof_; ++index) + { + joint_interface[index].get().set_value(trajectory_point_interface[index]); + } + } }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 2157fdf2a6..c46b1c297f 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -147,7 +147,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 %ld:", joint_idx); + RCLCPP_ERROR(logger, "State tolerances failed for joint %lu:", joint_idx); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 3bd4873a31..f199262f07 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -18,11 +18,14 @@ #include #include +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/time.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" + namespace joint_trajectory_controller { using TrajectoryPointIter = std::vector::iterator; @@ -89,7 +92,10 @@ class Trajectory const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const rclcpp::Duration & period, + std::unique_ptr> & + joint_limiter); /** * Do interpolation between 2 states given a time in between their respective timestamps @@ -155,6 +161,7 @@ class Trajectory trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_; bool sampled_already_ = false; + trajectory_msgs::msg::JointTrajectoryPoint previous_state_; }; /** diff --git a/joint_trajectory_controller/joint_trajectory_plugin.xml b/joint_trajectory_controller/joint_trajectory_plugin.xml index 27930380ea..9e179bfdcc 100644 --- a/joint_trajectory_controller/joint_trajectory_plugin.xml +++ b/joint_trajectory_controller/joint_trajectory_plugin.xml @@ -1,7 +1,14 @@ - - The joint trajectory controller executes joint-space trajectories on a set of joints - + + The joint trajectory controller executes joint-space trajectories on a set of joints. + + + + + + + The cartesian trajectory generator executes cartesian-space trajectories on a set of input points. + diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index bc6e43a1d5..1a2c8e68f0 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.4.0 + 4.5.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl @@ -18,6 +18,8 @@ control_toolbox generate_parameter_library hardware_interface + joint_limits + nav_msgs pluginlib rclcpp rclcpp_lifecycle @@ -25,9 +27,12 @@ rsl tl_expected trajectory_msgs + tf2 + tf2_geometry_msgs ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp new file mode 100644 index 0000000000..9c73b56568 --- /dev/null +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -0,0 +1,460 @@ +// Copyright (c) 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "joint_trajectory_controller/cartesian_trajectory_generator.hpp" + +#include "controller_interface/helpers.hpp" +#include "eigen3/Eigen/Eigen" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "joint_trajectory_controller/trajectory.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ // utility + +void reset_twist_msg(geometry_msgs::msg::Twist & msg) +{ + msg.linear.x = std::numeric_limits::quiet_NaN(); + msg.linear.y = std::numeric_limits::quiet_NaN(); + msg.linear.z = std::numeric_limits::quiet_NaN(); + msg.angular.x = std::numeric_limits::quiet_NaN(); + msg.angular.y = std::numeric_limits::quiet_NaN(); + msg.angular.z = std::numeric_limits::quiet_NaN(); +} + +using ControllerReferenceMsg = + cartesian_trajectory_generator::CartesianTrajectoryGenerator::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg(ControllerReferenceMsg & msg) +{ + msg.transforms.resize(1); + msg.transforms[0].translation.x = std::numeric_limits::quiet_NaN(); + msg.transforms[0].translation.y = std::numeric_limits::quiet_NaN(); + msg.transforms[0].translation.z = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.x = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.y = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.z = std::numeric_limits::quiet_NaN(); + msg.transforms[0].rotation.w = std::numeric_limits::quiet_NaN(); + + msg.velocities.resize(1); + reset_twist_msg(msg.velocities[0]); + + msg.accelerations.resize(1); + reset_twist_msg(msg.accelerations[0]); +} + +void reset_controller_reference_msg(const std::shared_ptr & msg) +{ + reset_controller_reference_msg(*msg); +} + +void rpy_to_quaternion( + std::array & orientation_angles, geometry_msgs::msg::Quaternion & quaternion_msg) +{ + // convert quaternion to euler angles + tf2::Quaternion quaternion; + quaternion.setRPY(orientation_angles[0], orientation_angles[1], orientation_angles[2]); + quaternion_msg = tf2::toMsg(quaternion); +} +} // namespace + +namespace cartesian_trajectory_generator +{ +CartesianTrajectoryGenerator::CartesianTrajectoryGenerator() +: joint_trajectory_controller::JointTrajectoryController() +{ +} + +controller_interface::InterfaceConfiguration +CartesianTrajectoryGenerator::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::NONE; + return conf; +} + +controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + // topics QoS + auto qos_best_effort_history_depth_one = rclcpp::SystemDefaultsQoS(); + qos_best_effort_history_depth_one.keep_last(1); + qos_best_effort_history_depth_one.best_effort(); + auto subscribers_reliable_qos = rclcpp::SystemDefaultsQoS(); + subscribers_reliable_qos.keep_all(); + subscribers_reliable_qos.reliable(); + + // Reference Subscribers (reliable channel also for updates not to be missed) + ref_subscriber_ = get_node()->create_subscription( + "~/reference", qos_best_effort_history_depth_one, + std::bind(&CartesianTrajectoryGenerator::reference_callback, this, std::placeholders::_1)); + ref_subscriber_reliable_ = get_node()->create_subscription( + "~/reference_reliable", subscribers_reliable_qos, + std::bind(&CartesianTrajectoryGenerator::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg_reset = std::make_shared(); + reset_controller_reference_msg(msg_reset); + reference_world_.writeFromNonRT(msg_reset); + + // Odometry feedback + auto feedback_callback = [&](const std::shared_ptr msg) -> void + { feedback_.writeFromNonRT(msg); }; + feedback_subscriber_ = get_node()->create_subscription( + "~/feedback", qos_best_effort_history_depth_one, feedback_callback); + // initialize feedback to null pointer since it is used to determine if we have valid data or not + feedback_.writeFromNonRT(nullptr); + + // service QoS + auto services_qos = rclcpp::SystemDefaultsQoS(); // message queue depth + services_qos.keep_all(); + services_qos.reliable(); + services_qos.durability_volatile(); + + cart_publisher_ = get_node()->create_publisher( + "~/controller_state_cartesian", qos_best_effort_history_depth_one); + cart_state_publisher_ = std::make_unique(cart_publisher_); + + cart_state_publisher_->lock(); + cart_state_publisher_->msg_.dof_names = params_.joints; + cart_state_publisher_->msg_.reference_world.transforms.resize(1); + cart_state_publisher_->msg_.reference_world.velocities.resize(1); + cart_state_publisher_->msg_.reference_world.accelerations.resize(1); + cart_state_publisher_->msg_.reference_local.transforms.resize(1); + cart_state_publisher_->msg_.reference_local.velocities.resize(1); + cart_state_publisher_->msg_.reference_local.accelerations.resize(1); + cart_state_publisher_->msg_.feedback.transforms.resize(1); + cart_state_publisher_->msg_.feedback.velocities.resize(1); + cart_state_publisher_->msg_.feedback.accelerations.resize(1); + cart_state_publisher_->msg_.feedback_local.transforms.resize(1); + cart_state_publisher_->msg_.feedback_local.velocities.resize(1); + cart_state_publisher_->msg_.feedback_local.accelerations.resize(1); + cart_state_publisher_->msg_.error.transforms.resize(1); + cart_state_publisher_->msg_.error.velocities.resize(1); + cart_state_publisher_->msg_.error.accelerations.resize(1); + cart_state_publisher_->msg_.output_world.transforms.resize(1); + cart_state_publisher_->msg_.output_world.velocities.resize(1); + cart_state_publisher_->msg_.output_world.accelerations.resize(1); + cart_state_publisher_->msg_.output_local.transforms.resize(1); + cart_state_publisher_->msg_.output_local.velocities.resize(1); + cart_state_publisher_->msg_.output_local.accelerations.resize(1); + cart_state_publisher_->unlock(); + + return CallbackReturn::SUCCESS; +} + +void CartesianTrajectoryGenerator::reference_callback( + const std::shared_ptr msg) +{ + // store input ref for later use + reference_world_.writeFromNonRT(msg); + + // assume for now that we are working with trajectories with one point - we don't know exactly + // where we are in the trajectory before sampling - nevertheless this should work for the use case + auto new_traj_msg = std::make_shared(); + new_traj_msg->joint_names = params_.joints; + new_traj_msg->points.resize(1); + new_traj_msg->points[0].positions.resize( + params_.joints.size(), std::numeric_limits::quiet_NaN()); + new_traj_msg->points[0].velocities.resize( + params_.joints.size(), std::numeric_limits::quiet_NaN()); + if (msg->time_from_start.nanosec == 0) + { + new_traj_msg->points[0].time_from_start = rclcpp::Duration::from_seconds(0.01); + } + else + { + new_traj_msg->points[0].time_from_start = rclcpp::Duration::from_nanoseconds( + static_cast(msg->time_from_start.nanosec)); + } + + // just pass input into trajectory message + auto assign_value_from_input = [&]( + const double pos_from_msg, const double vel_from_msg, + const std::string & joint_name, const size_t index) + { + new_traj_msg->points[0].positions[index] = pos_from_msg; + new_traj_msg->points[0].velocities[index] = vel_from_msg; + if (std::isnan(pos_from_msg) && std::isnan(vel_from_msg)) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "Input position and velocity for %s is NaN", joint_name.c_str()); + } + }; + + assign_value_from_input( + msg->transforms[0].translation.x, msg->velocities[0].linear.x, params_.joints[0], 0); + assign_value_from_input( + msg->transforms[0].translation.y, msg->velocities[0].linear.y, params_.joints[1], 1); + assign_value_from_input( + msg->transforms[0].translation.z, msg->velocities[0].linear.z, params_.joints[2], 2); + assign_value_from_input( + msg->transforms[0].rotation.x, msg->velocities[0].angular.x, params_.joints[3], 3); + assign_value_from_input( + msg->transforms[0].rotation.y, msg->velocities[0].angular.y, params_.joints[4], 4); + assign_value_from_input( + msg->transforms[0].rotation.z, msg->velocities[0].angular.z, params_.joints[5], 5); + + topic_callback(new_traj_msg); +} + +controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( + const rclcpp_lifecycle::State &) +{ + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + // parse remaining parameters + default_tolerances_ = get_segment_tolerances(params_); + + // order all joints in the storage + for (const auto & interface : params_.command_interfaces) + { + auto it = + std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + auto index = std::distance(allowed_interface_types_.begin(), it); + if (!controller_interface::get_ordered_interfaces( + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, + interface.c_str(), joint_command_interface_[index].size()); + return controller_interface::CallbackReturn::ERROR; + } + } + // for (const auto & interface : state_interface_types_) + // { + // auto it = + // std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); + // auto index = std::distance(allowed_interface_types_.begin(), it); + // if (!controller_interface::get_ordered_interfaces( + // state_interfaces_, joint_names_, interface, joint_state_interface_[index])) + // { + // RCLCPP_ERROR( + // get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, + // interface.c_str(), joint_state_interface_[index].size()); + // return controller_interface::CallbackReturn::ERROR; + // } + // } + + current_trajectory_ = std::make_shared(); + new_trajectory_msg_.writeFromNonRT(std::shared_ptr()); + + subscriber_is_active_ = true; + + // Handle restart of controller by reading from commands if those are not NaN (a controller was + // running already) + trajectory_msgs::msg::JointTrajectoryPoint state; + resize_joint_trajectory_point(state, dof_); + if (!read_state_from_state_interfaces(state)) return CallbackReturn::ERROR; + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; + + // Handle restart of controller by reading from commands if those are not NaN + if (read_state_from_command_interfaces(state)) + { + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; + } + 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); + + // parse timeout parameter + if (params_.cmd_timeout > 0.0) + { + if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) + { + cmd_timeout_ = params_.cmd_timeout; + } + else + { + // deactivate timeout + RCLCPP_WARN( + get_node()->get_logger(), + "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout, + default_tolerances_.goal_time_tolerance); + cmd_timeout_ = 0.0; + } + } + else + { + cmd_timeout_ = 0.0; + } + + return CallbackReturn::SUCCESS; +} + +bool CartesianTrajectoryGenerator::read_state_from_state_interfaces(JointTrajectoryPoint & state) +{ + std::array orientation_angles; + const auto measured_state = *(feedback_.readFromRT()); + if (!measured_state) return false; + + tf2::Quaternion measured_q; + + if ( + std::isnan(measured_state->pose.pose.orientation.w) || + std::isnan(measured_state->pose.pose.orientation.x) || + std::isnan(measured_state->pose.pose.orientation.y) || + std::isnan(measured_state->pose.pose.orientation.z)) + { + // if any of the orientation is NaN, revert to previous orientation + measured_q.setRPY(state.positions[3], state.positions[4], state.positions[5]); + } + else + { + tf2::fromMsg(measured_state->pose.pose.orientation, measured_q); + } + tf2::Matrix3x3 m(measured_q); + m.getRPY(orientation_angles[0], orientation_angles[1], orientation_angles[2]); + + // Assign values from the hardware + // Position states always exist + // if any measured position is NaN, keep previous value + state.positions[0] = std::isnan(measured_state->pose.pose.position.x) + ? state.positions[0] + : measured_state->pose.pose.position.x; + state.positions[1] = std::isnan(measured_state->pose.pose.position.y) + ? state.positions[1] + : measured_state->pose.pose.position.y; + state.positions[2] = std::isnan(measured_state->pose.pose.position.z) + ? state.positions[2] + : measured_state->pose.pose.position.z; + state.positions[3] = orientation_angles[0]; + state.positions[4] = orientation_angles[1]; + state.positions[5] = orientation_angles[2]; + + // Convert measured twist which is in body frame to world frame since CTG/JTC expects state + // in world frame + + Eigen::Quaterniond q_body_in_world( + measured_q.w(), measured_q.x(), measured_q.y(), measured_q.z()); + + // if any measured linear velocity is NaN, set to zero + Eigen::Vector3d linear_vel_body( + std::isnan(measured_state->twist.twist.linear.x) ? 0.0 : measured_state->twist.twist.linear.x, + std::isnan(measured_state->twist.twist.linear.y) ? 0.0 : measured_state->twist.twist.linear.y, + std::isnan(measured_state->twist.twist.linear.z) ? 0.0 : measured_state->twist.twist.linear.z); + auto linear_vel_world = q_body_in_world * linear_vel_body; + + // if any measured angular velocity is NaN, set to zero + Eigen::Vector3d angular_vel_body( + std::isnan(measured_state->twist.twist.angular.x) ? 0.0 : measured_state->twist.twist.angular.x, + std::isnan(measured_state->twist.twist.angular.y) ? 0.0 : measured_state->twist.twist.angular.y, + std::isnan(measured_state->twist.twist.angular.z) ? 0.0 + : measured_state->twist.twist.angular.z); + auto angular_vel_world = q_body_in_world * angular_vel_body; + + state.velocities[0] = linear_vel_world[0]; + state.velocities[1] = linear_vel_world[1]; + state.velocities[2] = linear_vel_world[2]; + state.velocities[3] = angular_vel_world[0]; + state.velocities[4] = angular_vel_world[1]; + state.velocities[5] = angular_vel_world[2]; + + state.accelerations.clear(); + return true; +} + +void CartesianTrajectoryGenerator::publish_state( + const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, + const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) +{ + joint_trajectory_controller::JointTrajectoryController::publish_state( + time, desired_state, current_state, state_error); + + if (cart_state_publisher_->trylock()) + { + cart_state_publisher_->msg_.header.stamp = time; + cart_state_publisher_->msg_.reference_world = *(*reference_world_.readFromRT()); + + auto set_multi_dof_point = + [&]( + trajectory_msgs::msg::MultiDOFJointTrajectoryPoint & multi_dof_point, + const JointTrajectoryPoint & traj_point) + { + if (traj_point.positions.size() == 6) + { + multi_dof_point.transforms[0].translation.x = traj_point.positions[0]; + multi_dof_point.transforms[0].translation.y = traj_point.positions[1]; + multi_dof_point.transforms[0].translation.z = traj_point.positions[2]; + + std::array orientation_angles = { + traj_point.positions[3], traj_point.positions[4], traj_point.positions[5]}; + geometry_msgs::msg::Quaternion quaternion; + rpy_to_quaternion(orientation_angles, quaternion); + multi_dof_point.transforms[0].rotation = quaternion; + } + if (traj_point.velocities.size() == 6) + { + multi_dof_point.velocities[0].linear.x = traj_point.velocities[0]; + multi_dof_point.velocities[0].linear.y = traj_point.velocities[1]; + multi_dof_point.velocities[0].linear.z = traj_point.velocities[2]; + multi_dof_point.velocities[0].angular.x = traj_point.velocities[3]; + multi_dof_point.velocities[0].angular.y = traj_point.velocities[4]; + multi_dof_point.velocities[0].angular.z = traj_point.velocities[5]; + } + if (traj_point.accelerations.size() == 6) + { + multi_dof_point.accelerations[0].linear.x = traj_point.accelerations[0]; + multi_dof_point.accelerations[0].linear.y = traj_point.accelerations[1]; + multi_dof_point.accelerations[0].linear.z = traj_point.accelerations[2]; + multi_dof_point.accelerations[0].angular.x = traj_point.accelerations[3]; + multi_dof_point.accelerations[0].angular.y = traj_point.accelerations[4]; + multi_dof_point.accelerations[0].angular.z = traj_point.accelerations[5]; + } + }; + + set_multi_dof_point(cart_state_publisher_->msg_.feedback_local, current_state); + set_multi_dof_point(cart_state_publisher_->msg_.error, state_error); + set_multi_dof_point(cart_state_publisher_->msg_.output_world, desired_state); + + const auto measured_state = *(feedback_.readFromRT()); + cart_state_publisher_->msg_.feedback.transforms[0].translation.x = + measured_state->pose.pose.position.x; + cart_state_publisher_->msg_.feedback.transforms[0].translation.y = + measured_state->pose.pose.position.y; + cart_state_publisher_->msg_.feedback.transforms[0].translation.z = + measured_state->pose.pose.position.z; + cart_state_publisher_->msg_.feedback.transforms[0].rotation = + measured_state->pose.pose.orientation; + cart_state_publisher_->msg_.feedback.velocities[0] = measured_state->twist.twist; + + cart_state_publisher_->unlockAndPublish(); + } +} + +} // namespace cartesian_trajectory_generator + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + cartesian_trajectory_generator::CartesianTrajectoryGenerator, + controller_interface::ControllerInterface) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4a2a801e51..8f522b7642 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -51,6 +51,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); + + joint_limiter_loader_ = std::make_shared>( + "joint_limits", "joint_limits::JointLimiterInterface"); + RCLCPP_DEBUG(get_node()->get_logger(), "Available joint limiter classes:"); + for (const auto & available_class : joint_limiter_loader_->getDeclaredClasses()) + { + RCLCPP_DEBUG(get_node()->get_logger(), " %s", available_class.c_str()); + } } catch (const std::exception & e) { @@ -122,93 +130,92 @@ controller_interface::return_type JointTrajectoryController::update( } } - auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, size_t index, - const JointTrajectoryPoint & current, - const JointTrajectoryPoint & desired) - { - // error defined as the difference between current and desired - if (joints_angle_wraparound_[index]) - { - // if desired, the shortest_angular_distance is calculated, i.e., the error is - // normalized between -piget_trajectory_msg(); - auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); + // Check if a new trajectory message has been received from Non-RT threads + const auto current_trajectory_msg = current_trajectory_->get_trajectory_msg(); + 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_external_msg != *new_external_msg && + current_trajectory_msg != *new_external_msg && (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); // TODO(denis): Add here integration of position and velocity - traj_external_point_ptr_->update(*new_external_msg); + current_trajectory_->update(*new_external_msg); } - // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not - // changed, but its value only? - auto assign_interface_from_point = - [&](auto & joint_interface, const std::vector & trajectory_point_interface) - { - for (size_t index = 0; index < dof_; ++index) - { - joint_interface[index].get().set_value(trajectory_point_interface[index]); - } - }; - // current state update state_current_.time_from_start.set__sec(0); - read_state_from_state_interfaces(state_current_); + if (!read_state_from_state_interfaces(state_current_)) + { + return controller_interface::return_type::OK; + } // currently carrying out a trajectory if (has_active_trajectory()) { bool first_sample = false; // if sampling the first time, set the point before you sample - if (!traj_external_point_ptr_->is_sampled_already()) + if (!current_trajectory_->is_sampled_already()) { first_sample = true; + if (params_.open_loop_control) { - traj_external_point_ptr_->set_point_before_trajectory_msg(time, last_commanded_state_); + auto reset_flags = reset_dofs_flags_.readFromRT(); + for (size_t i = 0; i < dof_; ++i) + { + if (reset_flags->at(i).reset) + { + last_commanded_state_.positions[i] = std::isnan(reset_flags->at(i).position) + ? state_current_.positions[i] + : reset_flags->at(i).position; + RCLCPP_INFO_STREAM( + get_node()->get_logger(), command_joint_names_[i] + << ": last commanded state position reset to " + << last_commanded_state_.positions[i]); + if (has_velocity_state_interface_) + { + last_commanded_state_.velocities[i] = std::isnan(reset_flags->at(i).velocity) + ? state_current_.velocities[i] + : reset_flags->at(i).velocity; + } + if (has_acceleration_state_interface_) + { + last_commanded_state_.accelerations[i] = std::isnan(reset_flags->at(i).acceleration) + ? state_current_.accelerations[i] + : reset_flags->at(i).acceleration; + } + + reset_flags->at(i).reset = false; // reset flag in the buffer for one-shot execution + } + } + + if (fabs(last_commanded_time_.seconds()) < std::numeric_limits::epsilon()) + { + last_commanded_time_ = time; + } + current_trajectory_->set_point_before_trajectory_msg( + last_commanded_time_, last_commanded_state_); } else { - traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_); + current_trajectory_->set_point_before_trajectory_msg(time, state_current_); } } // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = traj_external_point_ptr_->sample( - time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + const bool valid_point = current_trajectory_->sample( + time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr, period, + joint_limiter_); if (valid_point) { - const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); + const rclcpp::Time traj_start = current_trajectory_->time_from_start(); // this is the time instance // - started with the first segment: when the first point will be reached (in the future) // - later: when the point of the current segment was reached @@ -220,7 +227,7 @@ controller_interface::return_type JointTrajectoryController::update( bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; - const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + const bool before_last_point = end_segment_itr != current_trajectory_->end(); // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances @@ -230,8 +237,8 @@ controller_interface::return_type JointTrajectoryController::update( { RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } // Check state/goal tolerance @@ -315,6 +322,7 @@ controller_interface::return_type JointTrajectoryController::update( // store the previous command. Used in open-loop control mode last_commanded_state_ = state_desired_; + last_commanded_time_ = time; } if (active_goal) @@ -334,6 +342,7 @@ controller_interface::return_type JointTrajectoryController::update( { auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED); + result->set__error_string("Aborted due to path tolerance violation"); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 @@ -342,17 +351,18 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } // check goal tolerance else if (!before_last_point) { if (!outside_goal_tolerance) { - auto res = std::make_shared(); - res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); - active_goal->setSucceeded(res); + auto result = std::make_shared(); + result->set__error_code(FollowJTrajAction::Result::SUCCESSFUL); + result->set__error_string("Goal successfully reached!"); + active_goal->setSucceeded(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); @@ -360,25 +370,27 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_success_trajectory_point()); } else if (!within_goal_time) { + const std::string error_string = "Aborted due to goal_time_tolerance exceeding by " + + std::to_string(time_difference) + " seconds"; + auto result = std::make_shared(); result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED); + result->set__error_string(error_string); active_goal->setAborted(result); // TODO(matthew-reynolds): Need a lock-free write here // See https://github.com/ros-controls/ros2_controllers/issues/168 rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN( - get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", - time_difference); + RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } } } @@ -387,16 +399,16 @@ controller_interface::return_type JointTrajectoryController::update( // we need to ensure that there is no pending goal -> we get a race condition otherwise RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + 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) { RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } // else, run another cycle while waiting for outside_goal_tolerance // to be satisfied (will stay in this state until new message arrives) @@ -408,17 +420,8 @@ controller_interface::return_type JointTrajectoryController::update( return controller_interface::return_type::OK; } -void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) +bool JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) { - auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) - { - for (size_t index = 0; index < dof_; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } - }; - // Assign values from the hardware // Position states always exist assign_point_from_interface(state.positions, joint_state_interface_[0]); @@ -443,21 +446,13 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory state.velocities.clear(); state.accelerations.clear(); } + return true; } bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) { bool has_values = true; - auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) - { - for (size_t index = 0; index < dof_; ++index) - { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); - } - }; - auto interface_has_values = [](const auto & joint_interface) { return std::find_if( @@ -608,13 +603,14 @@ void JointTrajectoryController::query_state_service( if (has_active_trajectory()) { TrajectoryPointConstIter start_segment_itr, end_segment_itr; - response->success = traj_external_point_ptr_->sample( + const rclcpp::Duration period = rclcpp::Duration::from_seconds(0.01); + response->success = current_trajectory_->sample( static_cast(request->time), interpolation_method_, state_requested, - start_segment_itr, end_segment_itr); + start_segment_itr, end_segment_itr, period, joint_limiter_); // If the requested sample time precedes the trajectory finish time respond as failure if (response->success) { - if (end_segment_itr == traj_external_point_ptr_->end()) + if (end_segment_itr == current_trajectory_->end()) { RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); response->success = false; @@ -702,7 +698,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( has_effort_command_interface_ = contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT); - // if there is only velocity or if there is effort command interface // then use also PID adapter use_closed_loop_pid_adapter_ = (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && @@ -724,6 +719,24 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( { const auto & gains = params_.gains.joints_map.at(params_.joints[i]); joints_angle_wraparound_[i] = gains.angle_wraparound; + RCLCPP_INFO( + get_node()->get_logger(), "Angle Wraparound for joint '%s' is '%s'", + params_.joints[i].c_str(), joints_angle_wraparound_[i] ? "true" : "false"); + } + + // Initialize joint limits + if (!params_.joint_limiter_type.empty()) + { + RCLCPP_INFO( + get_node()->get_logger(), "Using joint limiter plugin: '%s'", + params_.joint_limiter_type.c_str()); + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_->createUnmanagedInstance(params_.joint_limiter_type)); + joint_limiter_->init(command_joint_names_, get_node()); + } + else + { + RCLCPP_INFO(get_node()->get_logger(), "Not using joint limiter plugin as none defined."); } if (params_.state_interfaces.empty()) @@ -757,6 +770,16 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "'position' state interfaces are present"); return CallbackReturn::FAILURE; } + if ( + has_acceleration_command_interface_ && + (!has_velocity_command_interface_ || !has_position_command_interface_)) + { + RCLCPP_ERROR( + logger, + "'acceleration' command interface can only be used if 'velocity' and " + "'position' command interfaces are present"); + return CallbackReturn::FAILURE; + } // effort is always used alone so no need for size check if ( @@ -876,6 +899,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + // Initialize memory and RT buffer for DoF reset flags + std::vector reset_flags; + reset_flags.resize( + dof_, {false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); + reset_dofs_flags_.writeFromNonRT(reset_flags); + + reset_dofs_service_ = get_node()->create_service( + "~/reset_dofs", + std::bind(&JointTrajectoryController::reset_dofs_service_callback, this, _1, _2)); + return CallbackReturn::SUCCESS; } @@ -921,9 +955,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } } - traj_external_point_ptr_ = std::make_shared(); - traj_msg_external_point_ptr_.writeFromNonRT( - std::shared_ptr()); + current_trajectory_ = std::make_shared(); + new_trajectory_msg_.writeFromNonRT(std::shared_ptr()); subscriber_is_active_ = true; @@ -931,17 +964,19 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); + if (!read_state_from_state_interfaces(state)) return CallbackReturn::ERROR; + state_current_ = state; + state_desired_ = state; + last_commanded_state_ = state; + + // Handle restart of controller by reading from commands if those are not NaN if (read_state_from_command_interfaces(state)) { state_current_ = state; + state_desired_ = state; last_commanded_state_ = state; } - else - { - // Initialize current state storage from hardware - read_state_from_state_interfaces(state_current_); - read_state_from_state_interfaces(last_commanded_state_); - } + 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()); @@ -1020,7 +1055,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; - traj_external_point_ptr_.reset(); + current_trajectory_.reset(); return CallbackReturn::SUCCESS; } @@ -1054,7 +1089,7 @@ bool JointTrajectoryController::reset() } } - traj_external_point_ptr_.reset(); + current_trajectory_.reset(); return true; } @@ -1191,6 +1226,117 @@ void JointTrajectoryController::goal_accepted_callback( std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } +void JointTrajectoryController::reset_dofs_service_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + response->ok = true; + + if ( + (request->positions.size() != request->velocities.size()) || + (request->velocities.size() != request->accelerations.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Reset dofs service call has different values size for positions %ld, velocities %ld, " + "accelerations %ld", + request->positions.size(), request->velocities.size(), request->accelerations.size()); + response->ok = false; + return; + } + + if ((request->positions.size() > 0) && (request->names.size() != request->positions.size())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Reset dofs service call has names size %ld different than positions size %ld", + request->names.size(), request->positions.size()); + response->ok = false; + return; + } + + std::vector reset_flags_reset; + reset_flags_reset.resize( + dof_, {false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); + + // Here we read current reset dofs flags and clear it. This is done so we can add this new + // request to the existing reset flags. This logic prevents this new request from overwriting + // any previous request that hasn't been processed yet. The one assumption made here is that the + // current reset flags are not going to be processed between the two calls here to read and + // reset, which is a highly unlikely scenario. Even if it was, the behavior is fairly benign in + // that the dofs in the previous request will be reset twice. + auto reset_flags = *reset_dofs_flags_.readFromNonRT(); + reset_dofs_flags_.writeFromNonRT(reset_flags_reset); + + // add/update reset dofs flags from request + for (size_t i = 0; i < request->names.size(); ++i) + { + auto it = + std::find(command_joint_names_.begin(), command_joint_names_.end(), request->names[i]); + if (it != command_joint_names_.end()) + { + auto cmd_itf_index = std::distance(command_joint_names_.begin(), it); + double pos = (request->positions.size() != 0) ? request->positions[i] + : std::numeric_limits::quiet_NaN(); + + if (request->positions.size() != 0) + { + RCLCPP_INFO(get_node()->get_logger(), "Resetting dof '%s'", request->names[i].c_str()); + } + else + { + RCLCPP_INFO( + get_node()->get_logger(), "Resetting dof '%s' position to %f", request->names[i].c_str(), + pos); + } + double vel = (request->velocities.size() != 0) ? request->velocities[i] + : std::numeric_limits::quiet_NaN(); + double accel = (request->accelerations.size() != 0) + ? request->accelerations[i] + : std::numeric_limits::quiet_NaN(); + reset_flags[cmd_itf_index] = {true, pos, vel, accel}; + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), "Name '%s' is not command interface. Ignoring this entry.", + request->names[i].c_str()); + response->ok = false; + } + } + + reset_dofs_flags_.writeFromNonRT(reset_flags); +} + +void JointTrajectoryController::compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) const +{ + // error defined as the difference between current and desired + if (joints_angle_wraparound_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -pi trajectory_msg) const { @@ -1252,7 +1398,7 @@ void JointTrajectoryController::fill_partial_goal( } void JointTrajectoryController::sort_to_local_joint_order( - std::shared_ptr trajectory_msg) + std::shared_ptr trajectory_msg) const { // rearrange all points in the trajectory message based on mapping std::vector mapping_vector = mapping(trajectory_msg->joint_names, params_.joints); @@ -1267,7 +1413,9 @@ void JointTrajectoryController::sort_to_local_joint_order( if (to_remap.size() != mapping.size()) { RCLCPP_WARN( - get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + get_node()->get_logger(), + "Invalid input size for sorting. Values have size %zu and mapping size %zu", + to_remap.size(), mapping.size()); return to_remap; } static std::vector output(dof_, 0.0); @@ -1322,6 +1470,7 @@ bool JointTrajectoryController::validate_trajectory_point_field( bool JointTrajectoryController::validate_trajectory_msg( const trajectory_msgs::msg::JointTrajectory & trajectory) const { + // CHECK: Partial joint goals // If partial joints goals are not allowed, goal should specify all controller joints if (!params_.allow_partial_joints_goal) { @@ -1334,33 +1483,21 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + // CHECK: if joint names are provided if (trajectory.joint_names.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); return false; } - const auto trajectory_start_time = static_cast(trajectory.header.stamp); - // If the starting time it set to 0.0, it means the controller should start it now. - // Otherwise we check if the trajectory ends before the current time, - // in which case it can be ignored. - if (trajectory_start_time.seconds() != 0.0) + // CHECK: if provided trajectory has points + if (trajectory.points.empty()) { - auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) - { - trajectory_end_time += p.time_from_start; - } - if (trajectory_end_time < get_node()->now()) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received trajectory with non-zero start time (%f) that ends in the past (%f)", - trajectory_start_time.seconds(), trajectory_end_time.seconds()); - return false; - } + RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); + return false; } + // CHECK: If joint names are matching the joints defined for the controller for (size_t i = 0; i < trajectory.joint_names.size(); ++i) { const std::string & incoming_joint_name = trajectory.joint_names[i]; @@ -1375,6 +1512,7 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + // CHECK: if trajectory ends with non-zero velocity (when option is disabled) if (!params_.allow_nonzero_velocity_at_trajectory_end) { for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) @@ -1390,9 +1528,32 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + // CHECK: if trajectory end time is in the past (if start time defined) + const auto trajectory_start_time = static_cast(trajectory.header.stamp); + // If the starting time it set to 0.0, it means the controller should start it now. + // Otherwise we check if the trajectory ends before the current time, + // in which case it can be ignored. + if (trajectory_start_time.seconds() != 0.0) + { + auto trajectory_end_time = trajectory_start_time; + for (const auto & p : trajectory.points) + { + trajectory_end_time += p.time_from_start; + } + if (trajectory_end_time < get_node()->now()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received trajectory with non-zero start time (%f) that ends in the past (%f)", + trajectory_start_time.seconds(), trajectory_end_time.seconds()); + return false; + } + } + rclcpp::Duration previous_traj_time(0ms); for (size_t i = 0; i < trajectory.points.size(); ++i) { + // CHECK: if time of points in the trajectory is monotonous if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) { RCLCPP_ERROR( @@ -1406,6 +1567,8 @@ bool JointTrajectoryController::validate_trajectory_msg( const size_t joint_count = trajectory.joint_names.size(); const auto & points = trajectory.points; + + // CHECK: if all required data are provided in the trajectory // This currently supports only position, velocity and acceleration inputs if (params_.allow_integration_in_goal_trajectories) { @@ -1448,7 +1611,7 @@ bool JointTrajectoryController::validate_trajectory_msg( void JointTrajectoryController::add_new_trajectory_msg( const std::shared_ptr & traj_msg) { - traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); + new_trajectory_msg_.writeFromNonRT(traj_msg); } void JointTrajectoryController::preempt_active_goal() @@ -1481,7 +1644,7 @@ JointTrajectoryController::set_success_trajectory_point() { // set last command to be repeated at success, no matter if it has nonzero velocity or // acceleration - hold_position_msg_ptr_->points[0] = traj_external_point_ptr_->get_trajectory_msg()->points.back(); + hold_position_msg_ptr_->points[0] = current_trajectory_->get_trajectory_msg()->points.back(); hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0); // set flag, otherwise tolerances will be checked with success_trajectory_point too @@ -1534,7 +1697,7 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( bool JointTrajectoryController::has_active_trajectory() const { - return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); + return current_trajectory_ != nullptr && current_trajectory_->has_trajectory_msg(); } void JointTrajectoryController::update_pids() diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index a6358d1015..167a9cfb2e 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -2,7 +2,8 @@ joint_trajectory_controller: joints: { type: string_array, default_value: [], - description: "Names of joints used by the controller", + description: "Joint names to control and listen to", + read_only: true, validation: { unique<>: null, } @@ -10,7 +11,7 @@ joint_trajectory_controller: command_joints: { type: string_array, default_value: [], - description: "Names of the commanding joints used by the controller", + description: "Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names.", read_only: true, validation: { unique<>: null, @@ -19,7 +20,8 @@ joint_trajectory_controller: command_interfaces: { type: string_array, default_value: [], - description: "Names of command interfaces to claim", + description: "Command interfaces provided by the hardware interface for all joints", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration", "effort",]], @@ -29,7 +31,8 @@ joint_trajectory_controller: state_interfaces: { type: string_array, default_value: [], - description: "Names of state interfaces to claim", + description: "State interfaces provided by the hardware for all joints.", + read_only: true, validation: { unique<>: null, subset_of<>: [["position", "velocity", "acceleration",]], @@ -39,12 +42,21 @@ joint_trajectory_controller: allow_partial_joints_goal: { type: bool, default_value: false, - description: "Goals with partial set of joints are allowed", + description: "Allow joint goals defining trajectory for only some joints.", } open_loop_control: { type: bool, default_value: false, - description: "Run the controller in open-loop, i.e., read hardware states only when starting controller. This is useful when robot is not exactly following the commanded trajectory.", + description: "Use controller in open-loop control mode + \n\n + * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation.\n + * It deactivates the feedback control, see the ``gains`` structure. + \n\n + This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). + \n\n + If this flag is set, the controller tries to read the values from the command interfaces on activation. + If they have real numeric values, those will be used instead of state interfaces. + Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits::quiet_NaN()``) or state values when the hardware is started.\n", read_only: true, } allow_integration_in_goal_trajectories: { @@ -55,7 +67,7 @@ joint_trajectory_controller: action_monitor_rate: { type: double, default_value: 20.0, - description: "Rate status changes will be monitored", + description: "Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory)", read_only: true, validation: { gt_eq: [0.1] @@ -80,7 +92,7 @@ joint_trajectory_controller: default_value: 0.0, # seconds description: "Timeout after which the input command is considered stale. Timeout is counted from the end of the trajectory (the last point). - cmd_timeout must be greater than constraints.goal_time, otherwise ignored. + ``cmd_timeout`` must be greater than ``constraints.goal_time``, otherwise ignored. If zero, timeout is deactivated", } gains: @@ -88,17 +100,17 @@ joint_trajectory_controller: p: { type: double, default_value: 0.0, - description: "Proportional gain for PID" + description: "Proportional gain :math:`k_p` for PID" } i: { type: double, default_value: 0.0, - description: "Integral gain for PID" + description: "Integral gain :math:`k_i` for PID" } d: { type: double, default_value: 0.0, - description: "Derivative gain for PID" + description: "Derivative gain :math:`k_d` for PID" } i_clamp: { type: double, @@ -108,13 +120,16 @@ joint_trajectory_controller: ff_velocity_scale: { type: double, default_value: 0.0, - description: "Feed-forward scaling of velocity." + description: "Feed-forward scaling :math:`k_{ff}` of velocity" } angle_wraparound: { type: bool, default_value: false, - description: "For joints that wrap around (ie. are continuous). - Normalizes position-error to -pi to pi." + description: 'For joints that wrap around (without end stop, ie. are continuous), + where the shortest rotation to the target position is the desired motion. + If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. + Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured + position :math:`s` from the state interface.' } constraints: stopped_velocity_tolerance: { @@ -142,3 +157,11 @@ joint_trajectory_controller: default_value: 0.0, description: "Per-joint trajectory offset tolerance at the goal position.", } + + joint_limiter_type: { + type: string, + default_value: "", + description: "The type of joint limiter to use. If empty, no joint limiter will be used. + Per default, the simple joint limiter implementation from 'joint_limits' package is used. + For other official implementations see 'joint_limiters' package in 'ros2_control' repository.", + } diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index fae4c41ff9..ffcd93dfcd 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -23,11 +23,15 @@ namespace joint_trajectory_controller { -Trajectory::Trajectory() : trajectory_start_time_(0), time_before_traj_msg_(0) {} +Trajectory::Trajectory() +: trajectory_start_time_(0), time_before_traj_msg_(0), sampled_already_(false) +{ +} Trajectory::Trajectory(std::shared_ptr joint_trajectory) : trajectory_msg_(joint_trajectory), - trajectory_start_time_(static_cast(joint_trajectory->header.stamp)) + trajectory_start_time_(static_cast(joint_trajectory->header.stamp)), + sampled_already_(false) { } @@ -48,6 +52,7 @@ void Trajectory::set_point_before_trajectory_msg( { time_before_traj_msg_ = current_time; state_before_traj_msg_ = current_point; + previous_state_ = current_point; } void Trajectory::update(std::shared_ptr joint_trajectory) @@ -61,7 +66,9 @@ bool Trajectory::sample( const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const rclcpp::Duration & period, + std::unique_ptr> & joint_limiter) { THROW_ON_NULLPTR(trajectory_msg_) @@ -89,7 +96,9 @@ bool Trajectory::sample( return false; } + // TODO(anyone): this shouldn't be initialized at runtime output_state = trajectory_msgs::msg::JointTrajectoryPoint(); + auto & first_point_in_msg = trajectory_msg_->points[0]; const rclcpp::Time first_point_timestamp = trajectory_start_time_ + first_point_in_msg.time_from_start; @@ -104,7 +113,7 @@ bool Trajectory::sample( } else { - // it changes points only if position and velocity do not exist, but their derivatives + // it changes a point only if position and velocity do not exist, but their derivatives deduce_from_derivatives( state_before_traj_msg_, first_point_in_msg, state_before_traj_msg_.positions.size(), (first_point_timestamp - time_before_traj_msg_).seconds()); @@ -115,6 +124,12 @@ bool Trajectory::sample( } start_segment_itr = begin(); // no segments before the first end_segment_itr = begin(); + + if (joint_limiter) + { + joint_limiter->enforce(state_before_traj_msg_, output_state, period); + } + previous_state_ = output_state; return true; } @@ -128,7 +143,7 @@ bool Trajectory::sample( const rclcpp::Time t0 = trajectory_start_time_ + point.time_from_start; const rclcpp::Time t1 = trajectory_start_time_ + next_point.time_from_start; - if (sample_time >= t0 && sample_time < t1) + if (sample_time >= t0 && sample_time <= t1) { // If interpolation is disabled, just forward the next waypoint if (interpolation_method == interpolation_methods::InterpolationMethod::NONE) @@ -139,30 +154,94 @@ bool Trajectory::sample( else { // it changes points only if position and velocity do not exist, but their derivatives - deduce_from_derivatives( - point, next_point, state_before_traj_msg_.positions.size(), (t1 - t0).seconds()); + deduce_from_derivatives(point, next_point, point.positions.size(), (t1 - t0).seconds()); interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); } start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); + + if (joint_limiter) + { + joint_limiter->enforce(previous_state_, output_state, period); + } + previous_state_ = output_state; return true; } } + // whole animation has played out - but still continue s interpolating and smoothing + auto & last_point = trajectory_msg_->points[trajectory_msg_->points.size() - 1]; + + // FIXME(destogl): this is from backport - check if needed + // + // // TODO: Add and test enforceJointLimits? Unsure if needed for end of animation + // // Yes, call enforceJointLimits to handle halting in servo, which has time_from_start == 1ns + // (does not enforce vel/acc limits) if(last_idx == 0) { + // // Enforce limits from current state, not the trajectory's single point, because the point + // from servo halting violates limits if (joint_limiter) + // { + // joint_limiter->enforce( + // state_before_traj_msg_, expected_state, (sample_time - time_before_traj_msg_)); + // } + // } + + const size_t dim = last_point.positions.size(); + + // the trajectories in msg may have empty velocities/accel, so resize them + if (last_point.velocities.empty()) + { + last_point.velocities.resize(dim, 0.0); + } + if (last_point.accelerations.empty()) + { + last_point.accelerations.resize(dim, 0.0); + } + + // integrate velocities and positions because acc and vel don't have to be 0 between points + for (size_t dim_i = 0; dim_i < dim; ++dim_i) + { + if (last_point.accelerations[dim_i] != 0) + { + last_point.velocities[dim_i] += last_point.accelerations[dim_i] * period.seconds(); + // remember velocity over multiple calls + } + if (last_point.velocities[dim_i] != 0) + { + last_point.positions[dim_i] += last_point.velocities[dim_i] * period.seconds(); + // remember velocity over multiple calls + } + } + // whole animation has played out start_segment_itr = --end(); end_segment_itr = end(); output_state = (*start_segment_itr); - // the trajectories in msg may have empty velocities/accel, so resize them - if (output_state.velocities.empty()) + + if (joint_limiter) { - output_state.velocities.resize(output_state.positions.size(), 0.0); + // When running Joint Limiter we might not get to the last_point in time - so SplineIt! + interpolate_between_points( + sample_time - period, previous_state_, sample_time, last_point, sample_time, output_state); + + // if limits are enforced time of the second point is in the future + if (joint_limiter->enforce(previous_state_, output_state, period)) + { + // TODO(destogl): spline it again to avoid oscillations in output from the filter + // interpolate_between_points( + // sample_time-period, previous_state_, sample_time + period, output_state, + // sample_time, output_state); + } + previous_state_ = output_state; } - if (output_state.accelerations.empty()) + else { - output_state.accelerations.resize(output_state.positions.size(), 0.0); + // OLD: the following 3 lines --> maybe to delete + const rclcpp::Time t0 = trajectory_start_time_ + last_point.time_from_start; + // // do not do splines when trajectory has finished because the time is achieved + interpolate_between_points(t0, last_point, t0, last_point, sample_time, output_state); } + return true; } @@ -175,6 +254,7 @@ void Trajectory::interpolate_between_points( rclcpp::Duration duration_btwn_points = time_b - time_a; const size_t dim = state_a.positions.size(); + // TODO(anyone): this shouldn't be resized at runtime output.positions.resize(dim, 0.0); output.velocities.resize(dim, 0.0); output.accelerations.resize(dim, 0.0); @@ -228,7 +308,6 @@ void Trajectory::interpolate_between_points( // do cubic interpolation double T[4]; generate_powers(3, duration_btwn_points.seconds(), T); - for (size_t i = 0; i < dim; ++i) { double start_pos = state_a.positions[i]; @@ -331,6 +410,33 @@ void Trajectory::deduce_from_derivatives( (first_state.velocities[i] + second_state.velocities[i]) * 0.5 * delta_t; } } + else + { + for (size_t i = 0; i < dim; ++i) + { + // reset velocities always to 0 if it is empty or NaN + double first_state_velocity = + first_state.velocities.empty() ? 0.0 : first_state.velocities[i]; + if (std::isnan(first_state_velocity)) + { + first_state.velocities[i] = 0.0; + first_state_velocity = 0.0; + } + double second_state_velocity = + second_state.velocities.empty() ? 0.0 : second_state.velocities[i]; + if (std::isnan(second_state_velocity)) + { + second_state.velocities[i] = 0.0; + second_state_velocity = 0.0; + } + + if (std::isnan(second_state.positions[i])) + { + second_state.positions[i] = + first_state.positions[i] + (first_state_velocity + second_state_velocity) * 0.5 * delta_t; + } + } + } } TrajectoryPointConstIter Trajectory::begin() const diff --git a/joint_trajectory_controller/test/test_load_cartesian_trajectory_generator.cpp b/joint_trajectory_controller/test/test_load_cartesian_trajectory_generator.cpp new file mode 100644 index 0000000000..84454b57f7 --- /dev/null +++ b/joint_trajectory_controller/test/test_load_cartesian_trajectory_generator.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "gmock/gmock.h" + +#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(TestLoadJointStateController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_cartesian_trajectory_generator", + "trajectory_interpolators/CartesianTrajectoryGenerator")); + + rclcpp::shutdown(); +} diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index b52aa67a04..f69f2dd62e 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -21,6 +21,7 @@ #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" +#include "joint_limits/joint_limiter_interface.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -36,6 +37,7 @@ namespace { // Floating-point value comparison threshold const double EPS = 1e-8; +std::unique_ptr> joint_limiter_; } // namespace TEST(TestTrajectory, initialize_trajectory) @@ -50,7 +52,10 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; - traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + traj.sample( + clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); + // traj.sample(rclcpp::Clock().now(), DEFAULT_INTERPOLATION, output_point, start, end, ); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -66,7 +71,9 @@ TEST(TestTrajectory, initialize_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_point; joint_trajectory_controller::TrajectoryPointConstIter start, end; - traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end); + traj.sample( + clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.end(), start); EXPECT_EQ(traj.end(), end); @@ -111,7 +118,9 @@ TEST(TestTrajectory, sample_trajectory_positions) // sample at trajectory starting time { - traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + traj.sample( + time_now, DEFAULT_INTERPOLATION, expected_state, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -123,7 +132,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(result, false); } @@ -131,7 +140,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); double half_current_to_p1 = (point_before_msg.positions[0] + p1.positions[0]) * 0.5; @@ -144,7 +153,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p1.positions[0], expected_state.positions[0], EPS); @@ -156,7 +165,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); double half_p1_to_p2 = (p1.positions[0] + p2.positions[0]) * 0.5; @@ -167,7 +176,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); double half_p2_to_p3 = (p2.positions[0] + p3.positions[0]) * 0.5; EXPECT_NEAR(half_p2_to_p3, expected_state.positions[0], EPS); } @@ -176,7 +185,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -184,9 +193,10 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, - start, end); + start, end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); + std::cout << "Expected state size: " << expected_state.positions.size() << std::endl; EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -195,7 +205,7 @@ TEST(TestTrajectory, sample_trajectory_positions) { traj.sample( time_now + rclcpp::Duration::from_seconds(30.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); @@ -352,7 +362,9 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) // sample at trajectory starting time { - traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + traj.sample( + time_now, DEFAULT_INTERPOLATION, expected_state, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -364,7 +376,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(result, false); } @@ -372,7 +384,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); double half_current_to_p1 = @@ -392,7 +404,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -406,7 +418,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); double half_p1_to_p2 = @@ -426,7 +438,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -440,7 +452,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); double half_p2_to_p3 = @@ -460,23 +472,23 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); EXPECT_NEAR(p3.velocities[0], expected_state.velocities[0], EPS); // the goal is reached so no acceleration anymore - EXPECT_NEAR(0, expected_state.accelerations[0], EPS); + EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); } - // sample past given points - movement virtually stops + // sample past given points - if there is velocity continue sampling { traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, - start, end); + start, end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); - EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); + EXPECT_NEAR(position_third_seg + p3.velocities[0] * 0.1, expected_state.positions[0], EPS); EXPECT_NEAR(p3.velocities[0], expected_state.velocities[0], EPS); EXPECT_NEAR(0.0, expected_state.accelerations[0], EPS); } @@ -525,7 +537,9 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho // sample at trajectory starting time { - traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + traj.sample( + time_now, DEFAULT_INTERPOLATION, expected_state, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -538,7 +552,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho { bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(result, false); } @@ -546,7 +560,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho { traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); // double half_current_to_p1 = point_before_msg.positions[0] + @@ -566,7 +580,7 @@ TEST(TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_witho { traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -617,7 +631,9 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) // sample at trajectory starting time { - traj.sample(time_now, DEFAULT_INTERPOLATION, expected_state, start, end); + traj.sample( + time_now, DEFAULT_INTERPOLATION, expected_state, start, end, + rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -630,7 +646,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) { bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(result, false); } @@ -644,7 +660,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ(traj.begin(), start); EXPECT_EQ((++traj.begin()), end); EXPECT_NEAR(position_first_seg, expected_state.positions[0], EPS); @@ -660,7 +676,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(2), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((++traj.begin()), start); EXPECT_EQ((--traj.end()), end); EXPECT_NEAR(position_second_seg, expected_state.positions[0], EPS); @@ -676,7 +692,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), DEFAULT_INTERPOLATION, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -688,7 +704,7 @@ TEST(TestTrajectory, sample_trajectory_acceleration_with_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), DEFAULT_INTERPOLATION, expected_state, - start, end); + start, end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_EQ((--traj.end()), start); EXPECT_EQ(traj.end(), end); EXPECT_NEAR(position_third_seg, expected_state.positions[0], EPS); @@ -734,7 +750,9 @@ TEST(TestTrajectory, skip_interpolation) // sample at trajectory starting time { - traj.sample(time_now, no_interpolation, expected_state, start, end); + traj.sample( + time_now, no_interpolation, expected_state, start, end, rclcpp::Duration::from_seconds(0.1), + joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); EXPECT_NEAR(point_before_msg.positions[0], expected_state.positions[0], EPS); @@ -749,7 +767,7 @@ TEST(TestTrajectory, skip_interpolation) { bool result = traj.sample( time_now - rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(result, false); } @@ -757,7 +775,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(0.5), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ(traj.begin(), end); // For passthrough, this should just return the first waypoint @@ -773,7 +791,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.0), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -788,7 +806,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(1.5), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ(traj.begin(), start); ASSERT_EQ((++traj.begin()), end); EXPECT_NEAR(p2.positions[0], expected_state.positions[0], EPS); @@ -798,7 +816,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(2.5), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -806,7 +824,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.0), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); } @@ -814,7 +832,7 @@ TEST(TestTrajectory, skip_interpolation) { traj.sample( time_now + rclcpp::Duration::from_seconds(3.125), no_interpolation, expected_state, start, - end); + end, rclcpp::Duration::from_seconds(0.1), joint_limiter_); ASSERT_EQ((--traj.end()), start); ASSERT_EQ(traj.end(), end); EXPECT_NEAR(p3.positions[0], expected_state.positions[0], EPS); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 72820c0295..24988618bb 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -120,13 +120,13 @@ TEST_P(TrajectoryControllerTestParameterized, activate) auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - auto cmd_interface_config = traj_controller_->command_interface_configuration(); - ASSERT_EQ( - cmd_interface_config.names.size(), joint_names_.size() * command_interface_types_.size()); + auto cmd_if_conf = traj_controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_names_.size() * command_interface_types_.size()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_interface_config = traj_controller_->state_interface_configuration(); - ASSERT_EQ( - state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); + auto state_if_conf = traj_controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_names_.size() * state_interface_types_.size()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state = ActivateTrajectoryController(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); @@ -461,6 +461,180 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) // Floating-point value comparison threshold const double EPS = 1e-6; + +/** + * @brief check if calculated trajectory error is correct with angle wraparound=true + */ +TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_true) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, 0.0, 1.0, true /* angle_wraparound */); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + std::vector> points_accelerations{ + {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}}; + // *INDENT-ON* + + trajectory_msgs::msg::JointTrajectoryPoint error, current, desired; + current.positions = {points[0].begin(), points[0].end()}; + current.velocities = {points_velocities[0].begin(), points_velocities[0].end()}; + current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()}; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + + // zero error + desired = current; + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + EXPECT_NEAR(error.positions[i], 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], 0., EPS); + } + } + + // angle wraparound of position error + desired.positions[0] += 3.0 * M_PI_2; + desired.velocities[0] += 1.0; + desired.accelerations[0] += 1.0; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + if (i == 0) + { + EXPECT_NEAR( + error.positions[i], desired.positions[i] - current.positions[i] - 2.0 * M_PI, EPS); + } + else + { + EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS); + } + + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS); + } + } + + executor.cancel(); +} + +/** + * @brief check if calculated trajectory error is correct with angle wraparound=false + */ +TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_false) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController( + executor, params, true, 0.0, 1.0, false /* angle_wraparound */); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + std::vector> points_accelerations{ + {{0.1, 0.1, 0.1}}, {{0.5, 0.5, 0.5}}, {{0.6, 0.6, 0.6}}}; + // *INDENT-ON* + + trajectory_msgs::msg::JointTrajectoryPoint error, current, desired; + current.positions = {points[0].begin(), points[0].end()}; + current.velocities = {points_velocities[0].begin(), points_velocities[0].end()}; + current.accelerations = {points_accelerations[0].begin(), points_accelerations[0].end()}; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + + // zero error + desired = current; + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + EXPECT_NEAR(error.positions[i], 0., EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], 0., EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], 0., EPS); + } + } + + // no normalization of position error + desired.positions[0] += 3.0 * M_PI_4; + desired.velocities[0] += 1.0; + desired.accelerations[0] += 1.0; + traj_controller_->resize_joint_trajectory_point(error, n_joints); + for (size_t i = 0; i < n_joints; ++i) + { + traj_controller_->testable_compute_error_for_joint(error, i, current, desired); + EXPECT_NEAR(error.positions[i], desired.positions[i] - current.positions[i], EPS); + if ( + traj_controller_->has_velocity_state_interface() && + (traj_controller_->has_velocity_command_interface() || + traj_controller_->has_effort_command_interface())) + { + // expect: error.velocities = desired.velocities - current.velocities; + EXPECT_NEAR(error.velocities[i], desired.velocities[i] - current.velocities[i], EPS); + } + if ( + traj_controller_->has_acceleration_state_interface() && + traj_controller_->has_acceleration_command_interface()) + { + // expect: error.accelerations = desired.accelerations - current.accelerations; + EXPECT_NEAR(error.accelerations[i], desired.accelerations[i] - current.accelerations[i], EPS); + } + } + + executor.cancel(); +} + /** * @brief check if position error of revolute joints are angle_wraparound if not configured so */ @@ -1192,6 +1366,11 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) traj_msg.joint_names = {"bad_name"}; EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + // empty message + traj_msg = good_traj_msg; + traj_msg.points.clear(); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + // No position data traj_msg = good_traj_msg; traj_msg.points[0].positions.clear(); @@ -1228,8 +1407,41 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); } -/// With allow_integration_in_goal_trajectories parameter trajectory missing position or -/// velocities are accepted +/** + * @brief Test invalid velocity at trajectory end with parameter set to false + */ +TEST_P( + TrajectoryControllerTestParameterized, + expect_invalid_when_message_with_nonzero_end_velocity_and_when_param_false) +{ + rclcpp::Parameter nonzero_vel_parameters("allow_nonzero_velocity_at_trajectory_end", false); + rclcpp::executors::SingleThreadedExecutor executor; + SetUpAndActivateTrajectoryController(executor, {nonzero_vel_parameters}); + + trajectory_msgs::msg::JointTrajectory traj_msg; + traj_msg.joint_names = joint_names_; + traj_msg.header.stamp = rclcpp::Time(0); + + // empty message (no throw!) + ASSERT_NO_THROW(traj_controller_->validate_trajectory_msg(traj_msg)); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // Nonzero velocity at trajectory end! + traj_msg.points.resize(1); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].positions.resize(1); + traj_msg.points[0].positions = {1.0, 2.0, 3.0}; + traj_msg.points[0].velocities.resize(1); + traj_msg.points[0].velocities = {-1.0, -2.0, -3.0}; + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); +} + +/** + * @brief missing_positions_message_accepted Test mismatched joint and reference vector sizes + * + * @note With allow_integration_in_goal_trajectories parameter trajectory missing position or + * velocities are accepted + */ TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted) { rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index e636fad4b7..485cdff884 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -66,6 +66,8 @@ class TestableJointTrajectoryController return ret; } + rclcpp::NodeOptions define_custom_node_options() const override { return node_options_; } + /** * @brief wait_for_trajectory block until a new JointTrajectory is received. * Requires that the executor is not spinned elsewhere between the @@ -107,6 +109,13 @@ class TestableJointTrajectoryController void trigger_declare_parameters() { param_listener_->declare_params(); } + void testable_compute_error_for_joint( + JointTrajectoryPoint & error, const size_t index, const JointTrajectoryPoint & current, + const JointTrajectoryPoint & desired) + { + compute_error_for_joint(error, index, current, desired); + } + trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset() const { return last_commanded_state_; @@ -140,21 +149,41 @@ class TestableJointTrajectoryController bool has_trivial_traj() const { - return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg() == false; + return has_active_trajectory() && current_trajectory_->has_nontrivial_msg() == false; } bool has_nontrivial_traj() { - return has_active_trajectory() && traj_external_point_ptr_->has_nontrivial_msg(); + return has_active_trajectory() && current_trajectory_->has_nontrivial_msg(); } double get_cmd_timeout() { return cmd_timeout_; } + void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; } trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } + /** + * a copy of the private member function + */ + void resize_joint_trajectory_point( + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) + { + point.positions.resize(size, 0.0); + if (has_velocity_state_interface_) + { + point.velocities.resize(size, 0.0); + } + if (has_acceleration_state_interface_) + { + point.accelerations.resize(size, 0.0); + } + } + rclcpp::WaitSet joint_cmd_sub_wait_set_; + rclcpp::NodeOptions node_options_; }; class TrajectoryControllerTest : public ::testing::Test @@ -209,8 +238,10 @@ class TrajectoryControllerTest : public ::testing::Test parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_)); parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); + traj_controller_->set_node_options(node_options); - return traj_controller_->init(controller_name_, "", 0, "", node_options); + return traj_controller_->init( + controller_name_, "", 0, "", traj_controller_->define_custom_node_options()); } void SetPidParameters( diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index b492f15efa..40bfd292f5 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* [PID] Remove joint_jog include (`#975 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index a34dc9f87f..f7b8cc4491 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -35,7 +35,6 @@ #include "std_srvs/srv/set_bool.hpp" #include "control_msgs/msg/joint_controller_state.hpp" -#include "control_msgs/msg/joint_jog.hpp" #include "control_msgs/msg/pid_state.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/pid_controller/package.xml b/pid_controller/package.xml index dc7f9c13be..70c7bfa987 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.4.0 + 4.5.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl @@ -27,6 +27,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index f645738862..1946070bad 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -74,6 +74,11 @@ pid_controller: default_value: 0.0, description: "Lower integral clamp. Only used if antiwindup is activated." } + save_iterm: { + type: bool, + default_value: false, + description: "Enables save of integral term on pid reset. Integrator error will not be set to 0. This is useful for pid loops that are enabled/disabled with constant steady-state external disturbance." + } feedforward_gain: { type: double, default_value: 0.0, diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 11f703a1a4..39c37689f0 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -52,6 +52,7 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) 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_FALSE(controller_->params_.gains.dof_names_map[dof_name].save_iterm); ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.0); } ASSERT_EQ(controller_->params_.command_interface, command_interface_); @@ -61,34 +62,36 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) ASSERT_FALSE(controller_->params_.use_external_measured_states); } -TEST_F(PidControllerTest, check_exported_intefaces) +TEST_F(PidControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_interfaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_interfaces.names.size(), dof_command_values_.size()); - for (size_t i = 0; i < command_interfaces.names.size(); ++i) + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) { - EXPECT_EQ(command_interfaces.names[i], dof_names_[i] + "/" + command_interface_); + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); } + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), dof_state_values_.size()); size_t si_index = 0; for (const auto & interface : state_interfaces_) { for (const auto & dof_name : dof_names_) { - EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); ++si_index; } } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); size_t ri_index = 0; for (const auto & interface : state_interfaces_) { @@ -96,10 +99,9 @@ TEST_F(PidControllerTest, check_exported_intefaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); - EXPECT_EQ( - reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); ++ri_index; } } diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index ab32f5cb48..1c356263e7 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -146,7 +146,9 @@ class PidControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_pid_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); std::vector command_ifs; command_itfs_.reserve(dof_names_.size()); diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index e0d3051226..3e17e69286 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -49,34 +49,36 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.command_interface, command_interface_); } -TEST_F(PidControllerTest, check_exported_intefaces) +TEST_F(PidControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), dof_command_values_.size()); - for (size_t i = 0; i < command_intefaces.names.size(); ++i) + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) { - EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + command_interface_); + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); } + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), dof_state_values_.size()); size_t si_index = 0; for (const auto & interface : state_interfaces_) { for (const auto & dof_name : reference_and_state_dof_names_) { - EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); ++si_index; } } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); size_t ri_index = 0; for (const auto & interface : state_interfaces_) { @@ -84,10 +86,9 @@ TEST_F(PidControllerTest, check_exported_intefaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); - EXPECT_EQ( - reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); ++ri_index; } } diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 7c3392dae9..e62920bf7f 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ + 4.4.0 (2024-01-11) ------------------ diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b714afc89f..e67d3d8a46 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.4.0 + 4.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios @@ -17,6 +17,8 @@ ament_cmake_gmock controller_manager + hardware_interface_testing + hardware_interface ros2_control_test_assets diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 3b4f00be12..60bff556db 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -54,7 +54,8 @@ void JointGroupPositionControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupPositionControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_position_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_position_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index fe61039fdb..bc27b5e629 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupPositionController, load_controller) diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 77a365009a..09ce2703a7 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Let sphinx add parameter description with nested structures to documentation (`#652 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/range_sensor_broadcaster/doc/userdoc.rst b/range_sensor_broadcaster/doc/userdoc.rst index e35bec67ad..39fb98b3aa 100644 --- a/range_sensor_broadcaster/doc/userdoc.rst +++ b/range_sensor_broadcaster/doc/userdoc.rst @@ -11,5 +11,21 @@ The controller is a wrapper around ``RangeSensor`` semantic component (see ``con Parameters ^^^^^^^^^^^ +The Range Sensor Broadcaster uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= .. generate_parameter_library_details:: ../src/range_sensor_broadcaster_parameters.yaml + + +An example parameter file +========================= + +.. generate_parameter_library_default:: + ../src/range_sensor_broadcaster_parameters.yaml + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/range_sensor_broadcaster_params.yaml + :language: yaml diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index d8213112da..2d865c1d7f 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.4.0 + 4.5.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien @@ -22,7 +22,7 @@ ament_cmake_gmock controller_manager - hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 7b8e6d0e02..010f18c1a6 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -37,7 +37,8 @@ controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster( std::string broadcaster_name) { controller_interface::return_type result = controller_interface::return_type::ERROR; - result = range_broadcaster_->init(broadcaster_name, "", 0); + result = range_broadcaster_->init( + broadcaster_name, "", 0, "", range_broadcaster_->define_custom_node_options()); if (controller_interface::return_type::OK == result) { @@ -154,8 +155,10 @@ TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) // check interface configuration auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = range_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( range_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), @@ -164,8 +167,10 @@ TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) // check interface configuration cmd_if_conf = range_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = range_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); // did not change + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index bdb3c388dd..8c96dc0ac0 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ + 4.4.0 (2024-01-11) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 265adde781..d8477b3ccd 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.4.0 + 4.5.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index eab2bcfcb3..ab8b17ea7a 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ + 4.4.0 (2024-01-11) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index f34838f5ff..4c99d9c18a 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.4.0 + 4.5.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl 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 cb66f58468..c038ab8f5b 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 @@ -16,8 +16,8 @@ # import rclpy +from rclpy.duration import Duration from rclpy.node import Node -from builtin_interfaces.msg import Duration from rcl_interfaces.msg import ParameterDescriptor from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint @@ -29,7 +29,9 @@ def __init__(self): super().__init__("publisher_position_trajectory_controller") # Declare all parameters self.declare_parameter("controller_name", "position_trajectory_controller") - self.declare_parameter("wait_sec_between_publish", 6) + self.declare_parameter("wait_sec_between_publish", 6.0) + self.declare_parameter("repeat_the_same_goal", 1) + self.declare_parameter("goal_time_from_start", 4.0) self.declare_parameter("goal_names", ["pos1", "pos2"]) self.declare_parameter("joints", [""]) self.declare_parameter("check_starting_point", False) @@ -37,6 +39,8 @@ def __init__(self): # Read parameters controller_name = self.get_parameter("controller_name").value wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value + self.repeat_the_same_goal = self.get_parameter("repeat_the_same_goal").value + goal_time_from_start = Duration(seconds=self.get_parameter("goal_time_from_start").value) goal_names = self.get_parameter("goal_names").value self.joints = self.get_parameter("joints").value self.check_starting_point = self.get_parameter("check_starting_point").value @@ -73,9 +77,13 @@ def __init__(self): def get_sub_param(sub_param): param_name = name + "." + sub_param - self.declare_parameter(param_name, [float()]) + # use default value different than 0.0 + self.declare_parameter(param_name, [float(1234567890)]) param_value = self.get_parameter(param_name).value + if len(param_value) == 1 and param_value[0] == 1234567890: + param_value.clear() + float_values = [] if len(param_value) != len(self.joints): @@ -108,7 +116,7 @@ def get_sub_param(sub_param): one_ok = True if one_ok: - point.time_from_start = Duration(sec=4) + point.time_from_start = goal_time_from_start.to_msg() self.goals.append(point) self.get_logger().info(f'Goal "{name}" has definition {point}') @@ -135,21 +143,26 @@ def get_sub_param(sub_param): self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1) self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) - self.i = 0 + self.current_goal = 0 + self.repetition = 0 def timer_callback(self): if self.starting_point_ok: - self.get_logger().info(f"Sending goal {self.goals[self.i]}.") + self.get_logger().info(f"Sending goal {self.goals[self.current_goal]}.") traj = JointTrajectory() traj.joint_names = self.joints - traj.points.append(self.goals[self.i]) + traj.points.append(self.goals[self.current_goal]) self.publisher_.publish(traj) - self.i += 1 - self.i %= len(self.goals) + self.repetition += 1 + self.repetition %= self.repeat_the_same_goal + + if self.repetition == 0: + self.current_goal += 1 + self.current_goal %= len(self.goals) elif self.check_starting_point and not self.joint_state_msg_received: self.get_logger().warn( diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index d9c72db506..4c5afefe0a 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.4.0", + version="4.5.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index 0558e15d49..c143c7c6ad 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ + 4.4.0 (2024-01-11) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 29db146abc..0929c2aaa3 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.4.0 + 4.5.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 02f32191d5..399ca590f9 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="4.4.0", + version="4.5.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index ec986caddc..8d67b186b6 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 90cb82ac8e..66a968b71f 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.4.0 + 4.5.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 81075d1082..0217567a26 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -28,41 +28,43 @@ class SteeringControllersLibraryTest }; // checking if all interfaces, command, state and reference are exported as expected -TEST_F(SteeringControllersLibraryTest, check_exported_intefaces) +TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + state_if_conf.names[STATE_STEER_LEFT_WHEEL], controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 5caf347ac1..1284b72096 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -73,7 +73,7 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; class TestableSteeringControllersLibrary : public steering_controllers_library::SteeringControllersLibrary { - FRIEND_TEST(SteeringControllersLibraryTest, check_exported_intefaces); + FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces); FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); public: @@ -168,7 +168,9 @@ class SteeringControllersLibraryFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_steering_controllers_library") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index a0dccedfe5..289ff3e6b0 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* [tricycle_controller] Use generate_parameter_library (`#957 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 4f6d064dc8..cdc69f182a 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -11,6 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS builtin_interfaces controller_interface geometry_msgs + generate_parameter_library hardware_interface nav_msgs pluginlib @@ -29,6 +30,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(tricycle_controller_parameters + src/tricycle_controller_parameter.yaml +) + add_library(tricycle_controller SHARED src/tricycle_controller.cpp src/odometry.cpp @@ -40,6 +45,7 @@ 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}) pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) @@ -50,8 +56,7 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_tricycle_controller - test/test_tricycle_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) + test/test_tricycle_controller.cpp) target_link_libraries(test_tricycle_controller tricycle_controller ) @@ -66,8 +71,9 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock(test_load_tricycle_controller + add_rostest_with_parameters_gmock(test_load_tricycle_controller test/test_load_tricycle_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml ) target_link_libraries(test_load_tricycle_controller tricycle_controller @@ -85,6 +91,7 @@ install( install( TARGETS tricycle_controller + tricycle_controller_parameters EXPORT export_tricycle_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index d153aeacba..991627eca2 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -5,7 +5,8 @@ tricycle_controller ===================== -Controller for mobile robots with tricycle drive. +Controller for mobile robots with a single double-actuated wheel, including traction and steering. An example is a tricycle robot with the actuated wheel in the front and two trailing wheels on the rear axle. + Input for control are robot base_link twist commands which are translated to traction and steering commands for the tricycle drive base. Odometry is computed from hardware feedback and published. @@ -24,3 +25,10 @@ Other features Odometry publishing Velocity, acceleration and jerk limits Automatic stop after command timeout + +Parameters +-------------- + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/tricycle_controller_parameter.yaml diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index cef90d026a..24aaf89de9 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -45,6 +45,9 @@ #include "tricycle_controller/traction_limiter.hpp" #include "tricycle_controller/visibility_control.h" +// auto-generated by generate_parameter_library +#include "tricycle_controller_parameters.hpp" + namespace tricycle_controller { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -109,31 +112,14 @@ class TricycleController : public controller_interface::ControllerInterface double convert_trans_rot_vel_to_steering_angle(double v, double omega, double wheelbase); std::tuple twist_to_ackermann(double linear_command, double angular_command); - std::string traction_joint_name_; - std::string steering_joint_name_; + // Parameters from ROS for tricycle_controller + std::shared_ptr param_listener_; + Params params_; // HACK: put into vector to avoid initializing structs because they have no default constructors std::vector traction_joint_; std::vector steering_joint_; - struct WheelParams - { - double wheelbase = 0.0; - double radius = 0.0; - } wheel_params_; - - struct OdometryParams - { - bool open_loop = false; - bool enable_odom_tf = false; - bool odom_only_twist = false; // for doing the pose integration in separate node - std::string base_frame_id = "base_link"; - std::string odom_frame_id = "odom"; - std::array pose_covariance_diagonal; - std::array twist_covariance_diagonal; - } odom_params_; - - bool publish_ackermann_command_ = false; std::shared_ptr> ackermann_command_publisher_ = nullptr; std::shared_ptr> realtime_ackermann_command_publisher_ = nullptr; @@ -168,7 +154,6 @@ class TricycleController : public controller_interface::ControllerInterface SteeringLimiter limiter_steering_; bool is_halted = false; - bool use_stamped_vel_ = true; void reset_odometry( const std::shared_ptr request_header, diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index cebd332a60..4a8725810b 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.4.0 + 4.5.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar @@ -10,6 +10,7 @@ Tony Najjar ament_cmake + generate_parameter_library ackermann_msgs backward_ros @@ -29,6 +30,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing ros2_control_test_assets diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index d6aeec0af1..07be8c2aae 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -52,41 +52,9 @@ CallbackReturn TricycleController::on_init() { try { - // with the lifecycle node being initialized, we can declare parameters - auto_declare("traction_joint_name", std::string()); - auto_declare("steering_joint_name", std::string()); - - auto_declare("wheelbase", wheel_params_.wheelbase); - auto_declare("wheel_radius", wheel_params_.radius); - - auto_declare("odom_frame_id", odom_params_.odom_frame_id); - auto_declare("base_frame_id", odom_params_.base_frame_id); - auto_declare>("pose_covariance_diagonal", std::vector()); - auto_declare>("twist_covariance_diagonal", std::vector()); - auto_declare("open_loop", odom_params_.open_loop); - auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); - auto_declare("odom_only_twist", odom_params_.odom_only_twist); - - auto_declare("cmd_vel_timeout", static_cast(cmd_vel_timeout_.count())); - auto_declare("publish_ackermann_command", publish_ackermann_command_); - auto_declare("velocity_rolling_window_size", 10); - auto_declare("use_stamped_vel", use_stamped_vel_); - - auto_declare("traction.max_velocity", NAN); - auto_declare("traction.min_velocity", NAN); - auto_declare("traction.max_acceleration", NAN); - auto_declare("traction.min_acceleration", NAN); - auto_declare("traction.max_deceleration", NAN); - auto_declare("traction.min_deceleration", NAN); - auto_declare("traction.max_jerk", NAN); - auto_declare("traction.min_jerk", NAN); - - auto_declare("steering.max_position", NAN); - auto_declare("steering.min_position", NAN); - auto_declare("steering.max_velocity", NAN); - auto_declare("steering.min_velocity", NAN); - auto_declare("steering.max_acceleration", NAN); - auto_declare("steering.min_acceleration", NAN); + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); } catch (const std::exception & e) { @@ -101,8 +69,8 @@ InterfaceConfiguration TricycleController::command_interface_configuration() con { InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); - command_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + command_interfaces_config.names.push_back(params_.traction_joint_name + "/" + HW_IF_VELOCITY); + command_interfaces_config.names.push_back(params_.steering_joint_name + "/" + HW_IF_POSITION); return command_interfaces_config; } @@ -110,8 +78,8 @@ InterfaceConfiguration TricycleController::state_interface_configuration() const { InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); - state_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + state_interfaces_config.names.push_back(params_.traction_joint_name + "/" + HW_IF_VELOCITY); + state_interfaces_config.names.push_back(params_.steering_joint_name + "/" + HW_IF_POSITION); return state_interfaces_config; } @@ -151,7 +119,7 @@ controller_interface::return_type TricycleController::update( 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 - if (odom_params_.open_loop) + if (params_.open_loop) { odometry_.updateOpenLoop(linear_command, angular_command, period); } @@ -172,7 +140,7 @@ controller_interface::return_type TricycleController::update( { auto & odometry_message = realtime_odometry_publisher_->msg_; odometry_message.header.stamp = time; - if (!odom_params_.odom_only_twist) + if (!params_.odom_only_twist) { odometry_message.pose.pose.position.x = odometry_.getX(); odometry_message.pose.pose.position.y = odometry_.getY(); @@ -186,7 +154,7 @@ controller_interface::return_type TricycleController::update( realtime_odometry_publisher_->unlockAndPublish(); } - if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); transform.header.stamp = time; @@ -239,7 +207,7 @@ controller_interface::return_type TricycleController::update( previous_commands_.emplace(ackermann_command); // Publish ackermann command - if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) + if (params_.publish_ackermann_command && realtime_ackermann_command_publisher_->trylock()) { 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 @@ -258,74 +226,40 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { auto logger = get_node()->get_logger(); - // update parameters - traction_joint_name_ = get_node()->get_parameter("traction_joint_name").as_string(); - steering_joint_name_ = get_node()->get_parameter("steering_joint_name").as_string(); - if (traction_joint_name_.empty()) + // update parameters if they have changed + if (param_listener_->is_old(params_)) { - RCLCPP_ERROR(logger, "'traction_joint_name' parameter was empty"); - return CallbackReturn::ERROR; + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); } - if (steering_joint_name_.empty()) - { - RCLCPP_ERROR(logger, "'steering_joint_name' parameter was empty"); - return CallbackReturn::ERROR; - } - - wheel_params_.wheelbase = get_node()->get_parameter("wheelbase").as_double(); - wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); - odometry_.setWheelParams(wheel_params_.wheelbase, wheel_params_.radius); - odometry_.setVelocityRollingWindowSize( - get_node()->get_parameter("velocity_rolling_window_size").as_int()); + odometry_.setWheelParams(params_.wheelbase, params_.wheel_radius); + odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); - odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); - odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); - - auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); - std::copy( - pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); - - auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); - std::copy( - twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); - - odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); - odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); - odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); - - cmd_vel_timeout_ = - std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()}; - publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); - use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout}; + params_.publish_ackermann_command = + get_node()->get_parameter("publish_ackermann_command").as_bool(); + params_.use_stamped_vel = get_node()->get_parameter("use_stamped_vel").as_bool(); try { limiter_traction_ = TractionLimiter( - get_node()->get_parameter("traction.min_velocity").as_double(), - get_node()->get_parameter("traction.max_velocity").as_double(), - get_node()->get_parameter("traction.min_acceleration").as_double(), - get_node()->get_parameter("traction.max_acceleration").as_double(), - get_node()->get_parameter("traction.min_deceleration").as_double(), - get_node()->get_parameter("traction.max_deceleration").as_double(), - get_node()->get_parameter("traction.min_jerk").as_double(), - get_node()->get_parameter("traction.max_jerk").as_double()); + params_.traction.min_velocity, params_.traction.max_velocity, + params_.traction.min_acceleration, params_.traction.max_acceleration, + params_.traction.min_deceleration, params_.traction.max_deceleration, + params_.traction.min_jerk, params_.traction.max_jerk); } catch (const std::invalid_argument & e) { RCLCPP_ERROR(get_node()->get_logger(), "Error configuring traction limiter: %s", e.what()); return CallbackReturn::ERROR; } - try { limiter_steering_ = SteeringLimiter( - get_node()->get_parameter("steering.min_position").as_double(), - get_node()->get_parameter("steering.max_position").as_double(), - get_node()->get_parameter("steering.min_velocity").as_double(), - get_node()->get_parameter("steering.max_velocity").as_double(), - get_node()->get_parameter("steering.min_acceleration").as_double(), - get_node()->get_parameter("steering.max_acceleration").as_double()); + params_.steering.min_position, params_.steering.max_position, params_.steering.min_velocity, + params_.steering.max_velocity, params_.steering.min_acceleration, + params_.steering.max_acceleration); } catch (const std::invalid_argument & e) { @@ -347,7 +281,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & previous_commands_.emplace(empty_ackermann_drive); // initialize ackermann command publisher - if (publish_ackermann_command_) + if (params_.publish_ackermann_command) { ackermann_command_publisher_ = get_node()->create_publisher( DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); @@ -357,7 +291,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } // initialize command subscriber - if (use_stamped_vel_) + if (params_.use_stamped_vel) { velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), @@ -409,8 +343,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odometry_publisher_); auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = odom_params_.odom_frame_id; - odometry_message.child_frame_id = odom_params_.base_frame_id; + odometry_message.header.frame_id = params_.odom_frame_id; + odometry_message.child_frame_id = params_.base_frame_id; // initialize odom values zeros odometry_message.twist = @@ -421,13 +355,12 @@ 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] = odom_params_.pose_covariance_diagonal[index]; - odometry_message.twist.covariance[diagonal_index] = - odom_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 - if (odom_params_.enable_odom_tf) + if (params_.enable_odom_tf) { odometry_transform_publisher_ = get_node()->create_publisher( DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); @@ -438,8 +371,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & // 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_params_.odom_frame_id; - odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; + odometry_transform_message.transforms.front().header.frame_id = params_.odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = params_.base_frame_id; } // Create odom reset service @@ -456,8 +389,8 @@ CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_node()->get_logger(), "On activate: Initialize Joints"); // Initialize the joints - const auto wheel_front_result = get_traction(traction_joint_name_, traction_joint_); - const auto steering_result = get_steering(steering_joint_name_, steering_joint_); + const auto wheel_front_result = get_traction(params_.traction_joint_name, traction_joint_); + const auto steering_result = get_steering(params_.steering_joint_name, steering_joint_); if (wheel_front_result == CallbackReturn::ERROR || steering_result == CallbackReturn::ERROR) { return CallbackReturn::ERROR; @@ -644,12 +577,12 @@ std::tuple TricycleController::twist_to_ackermann(double Vx, dou if (Vx == 0 && theta_dot != 0) { // is spin action alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(theta_dot) * wheel_params_.wheelbase / wheel_params_.radius; + Ws = abs(theta_dot) * params_.wheelbase / params_.wheel_radius; return std::make_tuple(alpha, Ws); } - alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_params_.wheelbase); - Ws = Vx / (wheel_params_.radius * std::cos(alpha)); + alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, params_.wheelbase); + Ws = Vx / (params_.wheel_radius * std::cos(alpha)); return std::make_tuple(alpha, Ws); } diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml new file mode 100644 index 0000000000..68fc2202c2 --- /dev/null +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -0,0 +1,149 @@ +tricycle_controller: + traction_joint_name: { + type: string, + default_value: "", + description: "Name of the traction joint", + validation: { + not_empty<>: [] + } + } + steering_joint_name: { + type: string, + default_value: "", + description: "Name of the steering joint", + validation: { + not_empty<>: [] + } + } + wheelbase: { + type: double, + default_value: 0.0, + description: "Shortest distance between the front wheel and the rear axle. If this parameter is wrong, the robot will not behave correctly in curves.", + } + wheel_radius: { + type: double, + default_value: 0.0, + description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + } + 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.", + } + base_frame_id: { + type: string, + default_value: "base_link", + description: "Name of the robot's base frame that is child of the odometry frame.", + } + pose_covariance_diagonal: { + 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]``.", + } + twist_covariance_diagonal: { + 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]``.", + } + 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.", + } + enable_odom_tf: { + type: bool, + default_value: false, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + } + odom_only_twist: { + type: bool, + default_value: false, + description: "for doing the pose integration in separate node.", + } + cmd_vel_timeout: { + type: int, + default_value: 500, # milliseconds + description: "Timeout in milliseconds, after which input command on ``cmd_vel`` topic is considered staled.", + } + publish_ackermann_command: { + type: bool, + default_value: false, + description: "Publish limited commands.", + } + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "Size of the rolling window for calculation of mean velocity use in odometry.", + validation: { + gt<>: [0] + } + } + use_stamped_vel: { + type: bool, + default_value: true, + description: "Use stamp from input velocity message to calculate how old the command actually is.", + } + traction: + # "The positive limit will be applied to both directions. Setting different limits for positive " + # "and negative directions is not supported. Actuators are " + # "assumed to have the same constraints in both directions" + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } + max_deceleration: { + type: double, + default_value: .NAN, + } + min_deceleration: { + type: double, + default_value: .NAN, + } + max_jerk: { + type: double, + default_value: .NAN, + } + min_jerk: { + type: double, + default_value: .NAN, + } + steering: + # "The positive limit will be applied to both directions. Setting different limits for positive " + # "and negative directions is not supported. Actuators are " + # "assumed to have the same constraints in both directions" + max_position: { + type: double, + default_value: .NAN, + } + min_position: { + type: double, + default_value: .NAN, + } + max_velocity: { + type: double, + default_value: .NAN, + } + min_velocity: { + type: double, + default_value: .NAN, + } + max_acceleration: { + type: double, + default_value: .NAN, + } + min_acceleration: { + type: double, + default_value: .NAN, + } diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 9298fae574..bd54459780 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -21,13 +21,13 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadTricycleController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -39,6 +39,13 @@ TEST(TestLoadTricycleController, load_controller) ASSERT_NE( cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); + return result; } diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index d8beedae42..5280aaf244 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -41,6 +41,12 @@ using lifecycle_msgs::msg::State; using testing::SizeIs; using testing::UnorderedElementsAre; +namespace +{ +const char traction_joint_name[] = "traction_joint"; +const char steering_joint_name[] = "steering_joint"; +} // namespace + class TestableTricycleController : public tricycle_controller::TricycleController { public: @@ -146,11 +152,28 @@ class TestTricycleController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + controller_interface::return_type InitController( + const std::string traction_joint_name_init = traction_joint_name, + const std::string steering_joint_name_init = steering_joint_name, + const std::vector & parameters = {}) + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name_init))); + parameter_overrides.push_back( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name_init))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name, urdf_, 0, "", node_options); + } + const std::string controller_name = "test_tricycle_controller"; std::unique_ptr controller_; - const std::string traction_joint_name = "traction_joint"; - const std::string steering_joint_name = "steering_joint"; double position_ = 0.1; double velocity_ = 0.2; @@ -172,42 +195,25 @@ class TestTricycleController : public ::testing::Test const std::string urdf_ = ""; }; -TEST_F(TestTricycleController, configure_fails_without_parameters) +TEST_F(TestTricycleController, init_fails_without_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + const auto ret = + controller_->init(controller_name, urdf_, 0, "", controller_->define_custom_node_options()); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); } -TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side_defined) +TEST_F(TestTricycleController, init_fails_if_only_traction_or_steering_side_defined) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(std::string()))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(std::string()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ( + InitController(traction_joint_name, std::string()), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ( + InitController(std::string(), steering_joint_name), controller_interface::return_type::ERROR); } TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -215,26 +221,22 @@ TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) auto cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); ASSERT_THAT( - cmd_if_conf.names, - UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + cmd_if_conf.names, UnorderedElementsAre( + std::string(traction_joint_name) + "/velocity", + std::string(steering_joint_name) + "/position")); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); ASSERT_THAT( - state_if_conf.names, - UnorderedElementsAre(traction_joint_name + "/velocity", steering_joint_name + "/position")); + state_if_conf.names, UnorderedElementsAre( + std::string(traction_joint_name) + "/velocity", + std::string(steering_joint_name) + "/position")); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestTricycleController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -242,15 +244,9 @@ TEST_F(TestTricycleController, activate_fails_without_resources_assigned) TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); // We implicitly test that by default position feedback is required - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResources(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -258,15 +254,11 @@ TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) TEST_F(TestTricycleController, cleanup) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 1.2)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.12)); + ASSERT_EQ( + InitController( + traction_joint_name, steering_joint_name, + {rclcpp::Parameter("wheelbase", 1.2), rclcpp::Parameter("wheel_radius", 0.12)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -307,15 +299,11 @@ TEST_F(TestTricycleController, cleanup) TEST_F(TestTricycleController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name, urdf_, 0); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + ASSERT_EQ( + InitController( + traction_joint_name, steering_joint_name, + {rclcpp::Parameter("wheelbase", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 3be8c9ec03..4f8d2be9f4 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ +* Add tests for `interface_configuration_type` consistently (`#899 `_) +* Contributors: Christoph Fröhlich + 4.4.0 (2024-01-11) ------------------ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index ad1c07d396..16bfd522f7 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.4.0 + 4.5.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar @@ -29,7 +29,7 @@ ament_cmake_gmock controller_manager - hardware_interface + hardware_interface_testing ros2_control_test_assets diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 82ba924305..c555de53de 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -45,46 +45,47 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } -TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index c5f6985d4e..6a516691b8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -61,7 +61,7 @@ class TestableTricycleSteeringController : public tricycle_steering_controller::TricycleSteeringController { FRIEND_TEST(TricycleSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(TricycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(TricycleSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(TricycleSteeringControllerTest, activate_success); FRIEND_TEST(TricycleSteeringControllerTest, update_success); FRIEND_TEST(TricycleSteeringControllerTest, deactivate_success); @@ -146,7 +146,9 @@ class TricycleSteeringControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") { - ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); if (position_feedback_ == true) { diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index dd72332875..6f2913aeb8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -47,46 +47,48 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } -TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], + cmd_if_conf.names[CMD_STEER_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 8cc9c869ef..cbf4e9d068 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.5.0 (2024-01-31) +------------------ + 4.4.0 (2024-01-11) ------------------ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 5319f55a97..3e28f7736e 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.4.0 + 4.5.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios @@ -17,6 +17,7 @@ ament_cmake_gmock controller_manager + hardware_interface_testing hardware_interface ros2_control_test_assets diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index 4cbf1b7342..a99ffaeebf 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -54,7 +54,8 @@ void JointGroupVelocityControllerTest::TearDown() { controller_.reset(nullptr); void JointGroupVelocityControllerTest::SetUpController() { - const auto result = controller_->init("test_joint_group_velocity_controller", "", 0); + const auto result = controller_->init( + "test_joint_group_velocity_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index 1872b5f746..e426349f96 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -17,7 +17,9 @@ #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" TEST(TestLoadJointGroupVelocityController, load_controller)