diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml deleted file mode 100644 index 357ee3dfa8..0000000000 --- a/.github/workflows/ci-coverage-build-humble.yml +++ /dev/null @@ -1,66 +0,0 @@ -name: Coverage Build - Humble -on: - workflow_dispatch: - push: - branches: - - humble - pull_request: - branches: - - humble - -jobs: - coverage: - name: coverage build - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - env: - ROS_DISTRO: humble - steps: - - uses: ros-tooling/setup-ros@0.7.1 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.6 - with: - target-ros2-distro: ${{ env.ROS_DISTRO }} - import-token: ${{ secrets.GITHUB_TOKEN }} - # build all packages listed here - 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 - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers - - vcs-repo-file-url: | - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} - colcon-defaults: | - { - "build": { - "mixin": ["coverage-gcc"] - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v4.0.1 - with: - file: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - - 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 deleted file mode 100644 index 7914a1acb0..0000000000 --- a/.github/workflows/ci-coverage-build-iron.yml +++ /dev/null @@ -1,66 +0,0 @@ -name: Coverage Build - Iron -on: - workflow_dispatch: - push: - branches: - - iron - pull_request: - branches: - - iron - -jobs: - coverage: - name: coverage build - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - env: - ROS_DISTRO: iron - steps: - - uses: ros-tooling/setup-ros@0.7.1 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.6 - with: - target-ros2-distro: ${{ env.ROS_DISTRO }} - import-token: ${{ secrets.GITHUB_TOKEN }} - # build all packages listed here - 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 - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers - - vcs-repo-file-url: | - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} - colcon-defaults: | - { - "build": { - "mixin": ["coverage-gcc"] - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v4.0.1 - with: - file: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - - 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 deleted file mode 100644 index b96276ca5a..0000000000 --- a/.github/workflows/ci-coverage-build.yml +++ /dev/null @@ -1,66 +0,0 @@ -name: Coverage Build -on: - workflow_dispatch: - push: - branches: - - master - pull_request: - branches: - - master - -jobs: - coverage: - name: coverage build - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - env: - ROS_DISTRO: rolling - steps: - - uses: ros-tooling/setup-ros@0.7.1 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.6 - with: - target-ros2-distro: ${{ env.ROS_DISTRO }} - import-token: ${{ secrets.GITHUB_TOKEN }} - # build all packages listed here - 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 - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers - - vcs-repo-file-url: | - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} - colcon-defaults: | - { - "build": { - "mixin": ["coverage-gcc"] - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v4.0.1 - with: - file: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - - uses: actions/upload-artifact@v4.3.1 - with: - name: colcon-logs-coverage-rolling - path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml deleted file mode 100644 index 824baf1b77..0000000000 --- a/.github/workflows/ci-format.yml +++ /dev/null @@ -1,22 +0,0 @@ -# This is a format job. Pre-commit has a first-party GitHub action, so we use -# that: https://github.com/pre-commit/action - -name: Format - -on: - pull_request: - -jobs: - pre-commit: - name: Format - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5.0.0 - with: - python-version: '3.10' - - name: Install system hooks - run: sudo apt install -qq clang-format-14 cppcheck - - uses: pre-commit/action@v3.0.1 - with: - extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml deleted file mode 100644 index df17d11dc4..0000000000 --- a/.github/workflows/ci-ros-lint.yml +++ /dev/null @@ -1,64 +0,0 @@ -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 }} - runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - linter: [cppcheck, copyright, lint_cmake] - env: - AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true - steps: - - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.1 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: rolling - linter: ${{ matrix.linter }} - package-name: ${{ env.package-name }} - - - ament_lint_100: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - linter: [cpplint] - steps: - - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@master - - uses: ros-tooling/action-ros-lint@master - with: - distribution: rolling - linter: cpplint - arguments: "--linelength=100 --filter=-whitespace/newline" - package-name: ${{ env.package-name }} diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 5c288fabfb..247371ba7d 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -4,6 +4,16 @@ on: pull_request: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-abi-compatibility.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.humble.repos' jobs: abi_check: diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index df449caecb..a031d823cf 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -7,9 +7,29 @@ on: pull_request: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.humble.repos' push: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.humble.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/humble-check-docs.yml b/.github/workflows/humble-check-docs.yml new file mode 100644 index 0000000000..0eeeaedf40 --- /dev/null +++ b/.github/workflows/humble-check-docs.yml @@ -0,0 +1,18 @@ +name: Humble Check Docs + +on: + workflow_dispatch: + pull_request: + branches: + - humble + paths: + - '**.rst' + - '**.md' + - '**.yaml' + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@humble + with: + ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml new file mode 100644 index 0000000000..d0dcb9c350 --- /dev/null +++ b/.github/workflows/humble-coverage-build.yml @@ -0,0 +1,38 @@ +name: Coverage Build - Humble +on: + workflow_dispatch: + push: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' + pull_request: + branches: + - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' + +jobs: + coverage_humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: humble diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index db0a8456f8..cd25248caf 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -4,6 +4,16 @@ on: pull_request: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml new file mode 100644 index 0000000000..5bb2408578 --- /dev/null +++ b/.github/workflows/humble-pre-commit.yml @@ -0,0 +1,13 @@ +name: Pre-Commit - Humble + +on: + workflow_dispatch: + pull_request: + branches: + - humble + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: humble diff --git a/.github/workflows/humble-rhel-semi-binary-build.yml b/.github/workflows/humble-rhel-semi-binary-build.yml index ccf64a0246..25c755ada5 100644 --- a/.github/workflows/humble-rhel-semi-binary-build.yml +++ b/.github/workflows/humble-rhel-semi-binary-build.yml @@ -4,6 +4,16 @@ on: pull_request: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-rhel-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index aaed9c8ca2..4246d18160 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -6,9 +6,29 @@ on: pull_request: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' push: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 7b4427d6d6..94af6edd8c 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -4,6 +4,16 @@ on: push: branches: - humble + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/humble-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.humble.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' @@ -15,3 +25,4 @@ jobs: ros_distro: humble ref: humble ros2_repo_branch: humble + os_name: ubuntu-22.04 diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml index ab6642625f..be1aec6680 100644 --- a/.github/workflows/iron-abi-compatibility.yml +++ b/.github/workflows/iron-abi-compatibility.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '.github/workflows/iron-abi-compatibility.yml' + - '**.yaml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.iron.repos' jobs: abi_check: diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml index 1510fac859..0d2f0ff7f3 100644 --- a/.github/workflows/iron-binary-build.yml +++ b/.github/workflows/iron-binary-build.yml @@ -9,9 +9,29 @@ on: - iron - '*feature*' - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/iron-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.iron.repos' push: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/iron-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.iron.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/ci-check-docs.yml b/.github/workflows/iron-check-docs.yml similarity index 57% rename from .github/workflows/ci-check-docs.yml rename to .github/workflows/iron-check-docs.yml index 629279615b..f842902055 100644 --- a/.github/workflows/ci-check-docs.yml +++ b/.github/workflows/iron-check-docs.yml @@ -1,12 +1,18 @@ -name: Check Docs +name: Iron Check Docs on: workflow_dispatch: pull_request: + branches: + - iron + paths: + - '**.rst' + - '**.md' + - '**.yaml' jobs: check-docs: name: Check Docs - uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@master + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@iron with: ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml new file mode 100644 index 0000000000..e1d2e84bc8 --- /dev/null +++ b/.github/workflows/iron-coverage-build.yml @@ -0,0 +1,38 @@ +name: Coverage Build - Iron +on: + workflow_dispatch: + push: + branches: + - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/iron-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' + pull_request: + branches: + - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/iron-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' + +jobs: + coverage_iron: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: iron diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml index e56e8940ad..ebd2f46c18 100644 --- a/.github/workflows/iron-debian-build.yml +++ b/.github/workflows/iron-debian-build.yml @@ -4,6 +4,16 @@ on: pull_request: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/iron-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml new file mode 100644 index 0000000000..a128958031 --- /dev/null +++ b/.github/workflows/iron-pre-commit.yml @@ -0,0 +1,13 @@ +name: Pre-Commit - Iron + +on: + workflow_dispatch: + pull_request: + branches: + - iron + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: iron diff --git a/.github/workflows/iron-rhel-semi-binary-build.yml b/.github/workflows/iron-rhel-semi-binary-build.yml index 66ad427a98..109dac7550 100644 --- a/.github/workflows/iron-rhel-semi-binary-build.yml +++ b/.github/workflows/iron-rhel-semi-binary-build.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/iron-rhel-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index 38ca4fe490..3aca5e5b70 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -8,9 +8,28 @@ on: - iron - '*feature*' - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/iron-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' push: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/iron-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml index 3609dcfc41..c54ead7877 100644 --- a/.github/workflows/iron-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -4,6 +4,16 @@ on: push: branches: - iron + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/iron-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.iron.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' @@ -15,3 +25,4 @@ jobs: ros_distro: iron ref: iron ros2_repo_branch: iron + os_name: ubuntu-22.04 diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml new file mode 100644 index 0000000000..8e557b5ee3 --- /dev/null +++ b/.github/workflows/jazzy-abi-compatibility.yml @@ -0,0 +1,27 @@ +name: Jazzy - ABI Compatibility Check +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-abi-compatibility.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.jazzy.repos' + +jobs: + abi_check: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: ros-industrial/industrial_ci@master + env: + ROS_DISTRO: jazzy + ROS_REPO: testing + ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} + NOT_TEST_BUILD: true diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml new file mode 100644 index 0000000000..cda4969abf --- /dev/null +++ b/.github/workflows/jazzy-binary-build.yml @@ -0,0 +1,49 @@ +name: Jazzy Binary Build +# author: Denis Štogl +# description: 'Build & test all dependencies from released (binary) packages.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.jazzy.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.jazzy.repos' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '03 1 * * *' + +jobs: + binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_REPO: [main, testing] + with: + ros_distro: jazzy + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers-not-released.jazzy.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/jazzy-check-docs.yml b/.github/workflows/jazzy-check-docs.yml new file mode 100644 index 0000000000..2f9eaf18bd --- /dev/null +++ b/.github/workflows/jazzy-check-docs.yml @@ -0,0 +1,18 @@ +name: Jazzy Check Docs + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.rst' + - '**.md' + - '**.yaml' + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@jazzy + with: + ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/jazzy-coverage-build.yml b/.github/workflows/jazzy-coverage-build.yml new file mode 100644 index 0000000000..c96ac88c32 --- /dev/null +++ b/.github/workflows/jazzy-coverage-build.yml @@ -0,0 +1,41 @@ +name: Coverage Build - Jazzy +on: + workflow_dispatch: + # TODO(anyone) activate when branched for Jazzy + # push: + # branches: + # - master + # paths: + # - '**.hpp' + # - '**.h' + # - '**.cpp' + # - '.github/workflows/jazzy-coverage-build.yml' + # - '**.yaml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'codecov.yml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'ros2_controllers.jazzy.repos' + # pull_request: + # branches: + # - master + # paths: + # - '**.hpp' + # - '**.h' + # - '**.cpp' + # - '.github/workflows/jazzy-coverage-build.yml' + # - '**.yaml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'codecov.yml' + # - '**/package.xml' + # - '**/CMakeLists.txt' + # - 'ros2_controllers.jazzy.repos' + +jobs: + coverage_jazzy: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: jazzy diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml new file mode 100644 index 0000000000..e3e3b8a353 --- /dev/null +++ b/.github/workflows/jazzy-debian-build.yml @@ -0,0 +1,30 @@ +name: Debian Rolling Source Build +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + jazzy_debian: + name: Rolling debian build + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master + with: + ros_distro: jazzy + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/jazzy-pre-commit.yml b/.github/workflows/jazzy-pre-commit.yml new file mode 100644 index 0000000000..d9ec610bbc --- /dev/null +++ b/.github/workflows/jazzy-pre-commit.yml @@ -0,0 +1,13 @@ +name: Pre-Commit - Jazzy + +on: + workflow_dispatch: + pull_request: + branches: + - master + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: jazzy diff --git a/.github/workflows/jazzy-rhel-semi-binary-build.yml b/.github/workflows/jazzy-rhel-semi-binary-build.yml new file mode 100644 index 0000000000..1c62fcf2ac --- /dev/null +++ b/.github/workflows/jazzy-rhel-semi-binary-build.yml @@ -0,0 +1,29 @@ +name: RHEL Jazzy Semi-Binary Build +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-rhel-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + jazzy_rhel: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master + with: + ros_distro: jazzy + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: master + skip_packages: rqt_joint_trajectory_controller diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml new file mode 100644 index 0000000000..ffa4f914b9 --- /dev/null +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -0,0 +1,49 @@ +name: Jazzy Semi-Binary Build +# description: 'Build & test that compiles the main dependencies from source.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + schedule: + # Run every morning to detect flakiness and broken dependencies + - cron: '33 1 * * *' + +jobs: + semi_binary: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_REPO: [main, testing] + with: + ros_distro: jazzy + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers.jazzy.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/jazzy-source-build.yml b/.github/workflows/jazzy-source-build.yml new file mode 100644 index 0000000000..45f6d1d25e --- /dev/null +++ b/.github/workflows/jazzy-source-build.yml @@ -0,0 +1,28 @@ +name: Jazzy Source Build +on: + workflow_dispatch: + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/jazzy-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.jazzy.repos' + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 3 * * *' + +jobs: + source: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master + with: + ros_distro: jazzy + ref: master + ros2_repo_branch: jazzy + container: ubuntu:24.04 diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 73055ef3e5..da3337554c 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-abi-compatibility.yml @@ -4,6 +4,15 @@ on: pull_request: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.yaml' + - '.github/workflows/rolling-abi-compatibility.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.rolling.repos' jobs: abi_check: diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index d0be23f076..b512eb9db7 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -9,9 +9,29 @@ on: - master - '*feature*' - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.rolling.repos' push: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers-not-released.rolling.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml new file mode 100644 index 0000000000..15b7eff57e --- /dev/null +++ b/.github/workflows/rolling-check-docs.yml @@ -0,0 +1,19 @@ +name: Rolling Check Docs + +on: + workflow_dispatch: + pull_request: + branches: + - master + paths: + - '**.rst' + - '**.md' + - '**.yaml' + - '.github/workflows/rolling-check-docs.yml' + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@rolling + with: + ROS2_CONTROLLERS_PR: ${{ github.ref }} diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml new file mode 100644 index 0000000000..058ff9bb33 --- /dev/null +++ b/.github/workflows/rolling-coverage-build.yml @@ -0,0 +1,38 @@ +name: Coverage Build - Rolling +on: + workflow_dispatch: + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + pull_request: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-coverage-build.yml' + - 'codecov.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + +jobs: + coverage_rolling: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: rolling diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index cecd67603f..d7efd3ea0a 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -4,6 +4,16 @@ on: pull_request: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-debian-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml new file mode 100644 index 0000000000..7bc07ba802 --- /dev/null +++ b/.github/workflows/rolling-pre-commit.yml @@ -0,0 +1,13 @@ +name: Pre-Commit - Rolling + +on: + workflow_dispatch: + pull_request: + branches: + - master + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: rolling diff --git a/.github/workflows/rolling-rhel-semi-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml index a175d951b4..29a53b810a 100644 --- a/.github/workflows/rolling-rhel-semi-binary-build.yml +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -4,6 +4,16 @@ on: pull_request: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-rhel-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 4784654db8..872c509931 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -8,9 +8,29 @@ on: - master - '*feature*' - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' push: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-semi-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 567b2c8ec6..e19a1e8cf7 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -4,6 +4,16 @@ on: push: branches: - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-source-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' @@ -15,3 +25,4 @@ jobs: ros_distro: rolling ref: master ros2_repo_branch: rolling + container: ubuntu:24.04 diff --git a/.github/workflows/rosdoc2.yml b/.github/workflows/rosdoc2.yml new file mode 100644 index 0000000000..0771248e79 --- /dev/null +++ b/.github/workflows/rosdoc2.yml @@ -0,0 +1,14 @@ +name: rosdoc2 + +on: + workflow_dispatch: + pull_request: + paths: + - ros2_controllers/doc/** + - ros2_controllers/rosdoc2.yaml + - ros2_controllers/package.xml + + +jobs: + check: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rosdoc2.yml@master diff --git a/.github/workflows/update-pre-commit.yml b/.github/workflows/update-pre-commit.yml new file mode 100644 index 0000000000..6bedaa0c97 --- /dev/null +++ b/.github/workflows/update-pre-commit.yml @@ -0,0 +1,12 @@ +name: Auto Update pre-commit +# Update pre-commit config and create PR if changes are detected +# author: Christoph Fröhlich + +on: + workflow_dispatch: + schedule: + - cron: '0 0 1 * *' # Runs at 00:00, on day 1 of the month + +jobs: + auto_update_and_create_pr: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-update-pre-commit.yml@master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 32d194d73d..205e0f63ab 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,3 +1,4 @@ + # To use: # # pre-commit run -a @@ -15,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v5.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -25,61 +26,62 @@ repos: - id: check-symlinks - id: check-xml - id: check-yaml + args: ["--allow-multiple-documents"] - id: debug-statements - id: end-of-file-fixer - id: mixed-line-ending - id: trailing-whitespace + exclude_types: [rst] - id: fix-byte-order-marker + # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.3.1 + rev: v3.19.0 hooks: - id: pyupgrade args: [--py36-plus] - - repo: https://github.com/psf/black - rev: 22.12.0 - hooks: - - id: black - args: ["--line-length=99"] - # PyDocStyle - repo: https://github.com/PyCQA/pydocstyle - rev: 6.2.2 + rev: 6.3.0 hooks: - id: pydocstyle args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + - repo: https://github.com/psf/black + rev: 24.10.0 + hooks: + - id: black + args: ["--line-length=99"] + - repo: https://github.com/pycqa/flake8 - rev: 6.0.0 + rev: 7.1.1 hooks: - id: flake8 - args: ["--ignore=E501"] + args: ["--extend-ignore=E501"] # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.6 + rev: v19.1.3 hooks: - id: clang-format + args: ['-fallback-style=none', '-i'] - repo: local hooks: - id: ament_cppcheck name: ament_cppcheck description: Static code analysis of C/C++ files. - stages: [commit] - entry: ament_cppcheck + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - # Maybe use https://github.com/cpplint/cpplint instead - repo: local hooks: - id: ament_cpplint name: ament_cpplint description: Static code analysis of C/C++ files. - stages: [commit] entry: ament_cpplint language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ @@ -91,7 +93,6 @@ repos: - id: ament_lint_cmake name: ament_lint_cmake description: Check format of CMakeLists.txt files. - stages: [commit] entry: ament_lint_cmake language: system files: CMakeLists\.txt$ @@ -102,13 +103,13 @@ repos: - id: ament_copyright name: ament_copyright description: Check if copyright notice is available in all files. - stages: [commit] entry: ament_copyright language: system + exclude: .*/conf\.py$ # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.1.1 + rev: v1.1.2 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -125,8 +126,18 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.2 + rev: v2.3.0 hooks: - id: codespell - args: ['--write-changes', '--uri-ignore-words-list=ist'] + args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ + + - repo: https://github.com/python-jsonschema/check-jsonschema + rev: 0.29.4 + hooks: + - id: check-github-workflows + args: ["--verbose"] + - id: check-github-actions + args: ["--verbose"] + - id: check-dependabot + args: ["--verbose"] diff --git a/README.md b/README.md index 7223ceae85..57a9ecdda1 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,8 @@ Commonly used and generalized controllers for ros2-control framework that are re ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_controllers/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) +**Rolling** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) +**Jazzy** | [`master`](https://github.com/ros-controls/ros2_controllers/tree/master) | [![Jazzy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-binary-build.yml?branch=master)
[![Jazzy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/jazzy-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/jazzy/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#jazzy) **Iron** | [`iron`](https://github.com/ros-controls/ros2_controllers/tree/iron) | [![Iron Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-binary-build.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/iron-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/iron/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#iron) **Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build.yml?branch=master) | [control.ros.org](https://control.ros.org/humble/doc/ros2_controllers/doc/controllers_index.html) | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 3b77b7b78f..bc2997b8a9 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,59 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ +* Add parameter check for geometric values (`#1120 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.7.0 (2024-03-22) +------------------ +* Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Chris Lalancette, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 71d0cf5f80..83edf7c157 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -24,6 +24,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(ackermann_steering_controller_parameters src/ackermann_steering_controller.yaml @@ -56,10 +58,8 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_ackermann_steering_controller - test/test_load_ackermann_steering_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_ackermann_steering_controller test/test_load_ackermann_steering_controller.cpp) ament_target_dependencies(test_load_ackermann_steering_controller controller_manager hardware_interface diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 512af88534..5764de56c1 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.6.0 + 4.15.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 @@ -16,6 +16,7 @@ generate_parameter_library + backward_ros control_msgs controller_interface hardware_interface diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index c3a7539c5a..d9d95bf8b5 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -62,28 +62,29 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio } else { - const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double front_right_steer_position = - state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); - const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); + const double traction_right_wheel_value = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double traction_left_wheel_value = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); + const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && - !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position)) + std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && + std::isfinite(steering_right_position) && std::isfinite(steering_left_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_right_position, + steering_left_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position, - front_left_steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_right_position, + steering_left_position, period.seconds()); } } } diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index 3726146919..1ec0b41c9f 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -5,6 +5,9 @@ ackermann_steering_controller: default_value: 0.0, description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } rear_wheel_track: @@ -13,6 +16,9 @@ ackermann_steering_controller: default_value: 0.0, description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } wheelbase: @@ -21,6 +27,9 @@ ackermann_steering_controller: default_value: 0.0, description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } front_wheels_radius: @@ -29,6 +38,9 @@ ackermann_steering_controller: default_value: 0.0, description: "Front wheels radius.", read_only: false, + validation: { + gt<>: [0.0] + } } rear_wheels_radius: @@ -37,4 +49,7 @@ ackermann_steering_controller: default_value: 0.0, description: "Rear wheels radius.", read_only: false, + validation: { + gt<>: [0.0] + } } diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index 6b64f901c3..6064814d32 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_ackermann_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steering_joint, front_left_steering_joint] diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml index ecb1b071e3..5d42adcdd5 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_ackermann_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] front_wheels_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint] rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index ef5454a16c..c434f73467 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_ackermann_steering_controller.hpp" -#include #include #include -#include #include class AckermannSteeringControllerTest @@ -84,16 +82,16 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + 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]); } } @@ -173,6 +171,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -211,8 +210,9 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR( + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( @@ -255,12 +255,13 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat EXPECT_EQ(msg.steering_angle_command[1], 4.4); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -276,6 +277,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat subscribe_and_get_messages(msg); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index a047186d14..59637a072f 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -15,22 +15,20 @@ #ifndef TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ #define TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include #include "ackermann_steering_controller/ackermann_steering_controller.hpp" -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using ControllerStateMsg = @@ -76,14 +74,7 @@ class TestableAckermannSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return ackermann_steering_controller::AckermannSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -97,30 +88,25 @@ class TestableAckermannSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -214,36 +200,43 @@ class AckermannSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_ackermann_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) @@ -280,7 +273,6 @@ class AckermannSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; @@ -303,7 +295,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; 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 1a16bed838..4f67beaa07 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_ackermann_steering_controller.hpp" -#include #include #include -#include #include class AckermannSteeringControllerTest @@ -93,9 +91,9 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { 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(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]); } } diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index 0a8cd7b80c..e7b56e2788 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -28,15 +28,16 @@ TEST(TestLoadAckermannSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/ackermann_steering_controller_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_ackermann_steering_controller", - "ackermann_steering_controller/AckermannSteeringController"), - nullptr); + cm.set_parameter({"test_ackermann_steering_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_ackermann_steering_controller.type", + "ackermann_steering_controller/AckermannSteeringController"}); + + ASSERT_NE(cm.load_controller("test_ackermann_steering_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 65db88f462..46899f9cd6 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,50 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fix segfault at reconfigure of AdmittanceController (`#1248 `_) +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Lennart Nachtigall, Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- +* Fix admittance controller interface read/write logic (`#1232 `_) +* Contributors: Nikola Banović + +4.12.0 (2024-07-23) +------------------- +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ +* Use CMake target for eigen (`#1058 `_) +* Contributors: Christoph Fröhlich + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index b2c10e0ba5..cd16714a31 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -11,7 +11,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs control_toolbox controller_interface - Eigen3 generate_parameter_library geometry_msgs hardware_interface @@ -34,6 +33,7 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(Eigen3 REQUIRED NO_MODULE) generate_parameter_library(admittance_controller_parameters src/admittance_controller_parameters.yaml @@ -49,6 +49,7 @@ target_include_directories(admittance_controller PUBLIC ) target_link_libraries(admittance_controller PUBLIC admittance_controller_parameters + Eigen3::Eigen ) ament_target_dependencies(admittance_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) @@ -67,10 +68,8 @@ if(BUILD_TESTING) find_package(kinematics_interface_kdl REQUIRED) # test loading admittance controller - add_rostest_with_parameters_gmock(test_load_admittance_controller - test/test_load_admittance_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp) ament_target_dependencies(test_load_admittance_controller controller_manager hardware_interface diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 6ff6cdae7a..9be6c3298c 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -17,7 +17,6 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_ -#include #include #include #include @@ -29,21 +28,14 @@ #include "admittance_controller/visibility_control.h" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "pluginlib/class_loader.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/force_torque_sensor.hpp" -#include "trajectory_msgs/msg/joint_trajectory.hpp" - namespace admittance_controller { using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 36c027491c..7223dbe9d1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -18,23 +18,16 @@ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ #include -#include -#include + #include #include #include +#include "admittance_controller_parameters.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" -#include "control_toolbox/filters.hpp" -#include "controller_interface/controller_interface.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "controller_interface/controller_interface_base.hpp" #include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" -#include "tf2_eigen/tf2_eigen.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_kdl/tf2_kdl.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 9b03924882..cab8b4cf45 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -22,9 +22,10 @@ #include #include +#include +#include + #include "rclcpp/duration.hpp" -#include "rclcpp/utilities.hpp" -#include "tf2_ros/transform_listener.h" namespace admittance_controller { @@ -45,11 +46,17 @@ controller_interface::return_type AdmittanceRule::configure( { try { + // Make sure we destroy the interface first. Otherwise we might run into a segfault + if (kinematics_loader_) + { + kinematics_.reset(); + } kinematics_loader_ = std::make_shared>( parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface"); kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); + if (!kinematics_->initialize( node->get_node_parameters_interface(), parameters_.kinematics.tip)) { diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 2c50dde2e7..06fde3cb59 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.6.0 + 4.15.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index c6a8168736..a4b56d739c 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -16,17 +16,13 @@ #include "admittance_controller/admittance_controller.hpp" -#include #include -#include #include #include #include #include "admittance_controller/admittance_rule_impl.hpp" #include "geometry_msgs/msg/wrench.hpp" -#include "rcutils/logging_macros.h" -#include "tf2_ros/buffer.h" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" namespace admittance_controller @@ -480,13 +476,13 @@ void AdmittanceController::read_state_from_hardware( state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value(); nan_position |= std::isnan(state_current.positions[joint_ind]); } - else if (has_velocity_state_interface_) + if (has_velocity_state_interface_) { state_current.velocities[joint_ind] = state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value(); nan_velocity |= std::isnan(state_current.velocities[joint_ind]); } - else if (has_acceleration_state_interface_) + if (has_acceleration_state_interface_) { state_current.accelerations[joint_ind] = state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); @@ -532,15 +528,15 @@ void AdmittanceController::write_state_to_hardware( command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value( state_commanded.positions[joint_ind]); } - else if (has_velocity_command_interface_) + if (has_velocity_command_interface_) { command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value( - state_commanded.positions[joint_ind]); + state_commanded.velocities[joint_ind]); } - else if (has_acceleration_command_interface_) + if (has_acceleration_command_interface_) { command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value( - state_commanded.positions[joint_ind]); + state_commanded.accelerations[joint_ind]); } } last_commanded_ = state_commanded; diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 6b03249df8..5c4bbe9438 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -16,9 +16,7 @@ #include "test_admittance_controller.hpp" -#include #include -#include #include // Test on_init returns ERROR when a required parameter is missing @@ -277,7 +275,7 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) // ASSERT_EQ(msg.wrench_base.header.frame_id, ik_base_frame_); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); broadcast_tfs(); ASSERT_EQ( diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 19908d7f9f..b2a95c12fa 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -17,6 +17,8 @@ #ifndef TEST_ADMITTANCE_CONTROLLER_HPP_ #define TEST_ADMITTANCE_CONTROLLER_HPP_ +#include + #include #include #include @@ -25,21 +27,18 @@ #include #include -#include "gmock/gmock.h" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "admittance_controller/admittance_controller.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/parameter_value.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "semantic_components/force_torque_sensor.hpp" #include "test_asset_6d_robot_description.hpp" #include "tf2_ros/transform_broadcaster.h" -#include "trajectory_msgs/msg/joint_trajectory.hpp" // TODO(anyone): replace the state and command message types using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped; @@ -57,15 +56,6 @@ constexpr auto NODE_FAILURE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; constexpr auto NODE_ERROR = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} - } // namespace // subclassing and friending so we can access member variables @@ -92,39 +82,27 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override { - auto ret = admittance_controller::AdmittanceController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - input_pose_command_subscriber_wait_set_.add_subscription(input_joint_command_subscriber_); - } - return ret; + return admittance_controller::AdmittanceController::on_configure(previous_state); } /** * @brief wait_for_commands blocks until a new ControllerCommandMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerCommandMsg msg was received, false if timeout. */ - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = - input_pose_command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } private: - rclcpp::WaitSet input_wrench_command_subscriber_wait_set_; - rclcpp::WaitSet input_pose_command_subscriber_wait_set_; const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf; const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf; }; @@ -279,9 +257,12 @@ class AdmittanceControllerTest : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + ControllerStateMsg::SharedPtr received_msg; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node_->create_subscription( "/test_admittance_controller/status", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node_->get_node_base_interface()); // call update to publish the test value ASSERT_EQ( @@ -289,11 +270,18 @@ class AdmittanceControllerTest : public ::testing::Test controller_interface::return_type::OK); // wait for message to be passed - ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node_->get_clock()->now() + timeout; + while (!received_msg && test_subscription_node_->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands() diff --git a/admittance_controller/test/test_asset_6d_robot_description.hpp b/admittance_controller/test/test_asset_6d_robot_description.hpp index 4d38df7c30..11412ebbab 100644 --- a/admittance_controller/test/test_asset_6d_robot_description.hpp +++ b/admittance_controller/test/test_asset_6d_robot_description.hpp @@ -15,8 +15,6 @@ #ifndef TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ #define TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ -#include - namespace ros2_control_test_assets { const auto valid_6d_robot_urdf = diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index 23be1f23f5..5006920858 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -29,13 +29,14 @@ TEST(TestLoadAdmittanceController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = std::string(TEST_FILES_DIRECTORY) + "/test_params.yaml"; - ASSERT_EQ( - cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"), - nullptr); + cm.set_parameter({"load_admittance_controller.params_file", test_file_path}); + cm.set_parameter( + {"load_admittance_controller.type", "admittance_controller/AdmittanceController"}); + + ASSERT_EQ(cm.load_controller("load_admittance_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 16192d9f8a..0d096951ca 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,59 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ +* Add parameter check for geometric values (`#1120 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.7.0 (2024-03-22) +------------------ +* Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Chris Lalancette, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 77d82ed874..db1f5cf4ba 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -24,6 +24,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(bicycle_steering_controller_parameters src/bicycle_steering_controller.yaml @@ -55,10 +57,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_bicycle_steering_controller - test/test_load_bicycle_steering_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_bicycle_steering_controller test/test_load_bicycle_steering_controller.cpp) ament_target_dependencies(test_load_bicycle_steering_controller controller_manager hardware_interface diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index bc560d9bf7..f58cf42dff 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.6.0 + 4.15.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar @@ -16,6 +16,7 @@ generate_parameter_library + backward_ros control_msgs controller_interface hardware_interface diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 5f013d7d7a..95eaf1965c 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -60,19 +60,19 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) } else { - const double rear_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); - const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); - if (!std::isnan(rear_wheel_value) && !std::isnan(steer_position)) + const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information - odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_position(traction_wheel_value, steering_position, period.seconds()); } else { // Estimate linear and angular velocity using joint information - odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + odometry_.update_from_velocity(traction_wheel_value, steering_position, period.seconds()); } } } diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.yaml b/bicycle_steering_controller/src/bicycle_steering_controller.yaml index c40e27ef96..fde323ef74 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.yaml +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -5,6 +5,9 @@ bicycle_steering_controller: default_value: 0.0, description: "Distance between front and rear wheel. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } front_wheel_radius: @@ -13,6 +16,9 @@ bicycle_steering_controller: default_value: 0.0, description: "Front wheel radius.", read_only: false, + validation: { + gt<>: [0.0] + } } rear_wheel_radius: @@ -21,4 +27,7 @@ bicycle_steering_controller: default_value: 0.0, description: "Rear wheel radius.", read_only: false, + validation: { + gt<>: [0.0] + } } diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml index a2a6c6508b..3a656cc724 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_bicycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_wheel_joint] front_wheels_names: [steering_axis_joint] diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml index 39ffeed878..e1b9c1ab72 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_bicycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_wheel_joint] front_wheels_names: [pid_controller/steering_axis_joint] rear_wheels_state_names: [rear_wheel_joint] diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 3dcdc0b1db..7ec0983f93 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_bicycle_steering_controller.hpp" -#include #include #include -#include #include class BicycleSteeringControllerTest @@ -68,16 +66,16 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + 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]); } } @@ -158,7 +156,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); @@ -190,7 +188,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); @@ -237,22 +235,22 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status EXPECT_EQ(msg.linear_velocity_command[0], 1.1); EXPECT_EQ(msg.steering_angle_command[0], 2.2); - publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + publish_commands(0.1, 0.2); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); - EXPECT_NEAR(msg.linear_velocity_command[0], 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR(msg.linear_velocity_command[0], 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); } diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 5e21ff228c..65f1691a3b 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -15,22 +15,20 @@ #ifndef TEST_BICYCLE_STEERING_CONTROLLER_HPP_ #define TEST_BICYCLE_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include #include "bicycle_steering_controller/bicycle_steering_controller.hpp" -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using ControllerStateMsg = @@ -74,13 +72,7 @@ class TestableBicycleSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return bicycle_steering_controller::BicycleSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -94,30 +86,25 @@ class TestableBicycleSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -187,34 +174,43 @@ class BicycleSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_bicycle_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on max_sub_check_loop_count - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) @@ -251,7 +247,6 @@ class BicycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_wheel_joint"}; std::vector front_wheels_names_ = {"steering_axis_joint"}; std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; @@ -267,7 +262,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {3.3, 0.5}; std::array joint_command_values_ = {1.1, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup 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 bc3a182753..0a321009c2 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_bicycle_steering_controller.hpp" -#include #include #include -#include #include class BicycleSteeringControllerTest @@ -79,9 +77,9 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { 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(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]); } } diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp index 955feb33c5..4d17a4bdd7 100644 --- a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -28,14 +28,16 @@ TEST(TestLoadBicycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/bicycle_steering_controller_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController"), - nullptr); + cm.set_parameter({"test_bicycle_steering_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_bicycle_steering_controller.type", + "bicycle_steering_controller/BicycleSteeringController"}); + + ASSERT_NE(cm.load_controller("test_bicycle_steering_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 1ec4329fca..4c48fb234d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,58 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* rename get/set_state to get/set_lifecylce_state (`#1250 `_) +* Contributors: Manuel Muth + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Add missing includes (`#1226 `_) +* Remove duplicated call to rclcpp::shutdown in test (`#1220 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Noel Jiménez García, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) +* Bump version of pre-commit hooks (`#1157 `_) +* Contributors: Christoph Fröhlich, github-actions[bot] + +4.8.0 (2024-05-14) +------------------ +* Remove non-existing parameter (`#1119 `_) +* Add parameter check for geometric values (`#1120 `_) +* Deprecate non-stamped twist for tricycle_controller and steering_controllers (`#1093 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.7.0 (2024-03-22) +------------------ +* added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Contributors: Sai Kishor Kothakota + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` also for diff_drive (`#1021 `_) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index d67815b5e0..4eeb98b9d2 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -25,6 +25,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(diff_drive_controller_parameters src/diff_drive_controller_parameter.yaml @@ -68,10 +70,8 @@ if(BUILD_TESTING) tf2_msgs ) - 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 - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp) ament_target_dependencies(test_load_diff_drive_controller controller_manager ros2_control_test_assets diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 70d0d7ca5c..29df4e55ba 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -11,6 +11,8 @@ As input it takes velocity commands for the robot body, which are translated to Odometry is computed from hardware feedback and published. +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. + Other features -------------- @@ -46,7 +48,7 @@ Subscribers ,,,,,,,,,,,, ~/cmd_vel [geometry_msgs/msg/TwistStamped] - Velocity command for the controller, if ``use_stamped_vel=true``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + Velocity command for the controller. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. Publishers 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 72b38f7d2d..ac34b81eca 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 @@ -30,15 +30,11 @@ #include "diff_drive_controller/odometry.hpp" #include "diff_drive_controller/speed_limiter.hpp" #include "diff_drive_controller/visibility_control.h" -#include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" #include "odometry.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "tf2_msgs/msg/tf_message.hpp" diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 83ab270f72..edca431d3d 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -25,7 +25,12 @@ #include #include "rclcpp/time.hpp" +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 #include "rcpputils/rolling_mean_accumulator.hpp" +#else +#include "rcppmath/rolling_mean_accumulator.hpp" +#endif namespace diff_drive_controller { @@ -50,7 +55,12 @@ class Odometry void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); private: +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; +#else + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; +#endif void integrateRungeKutta2(double linear, double angular); void integrateExact(double linear, double angular); diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index b0e818dd50..45ab80d5cb 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.6.0 + 4.15.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 42b6cda8e1..66da6d6738 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -101,7 +101,7 @@ controller_interface::return_type DiffDriveController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { auto logger = get_node()->get_logger(); - if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) { @@ -352,7 +352,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( received_velocity_msg_ptr_.set(std::move(msg)); }); - // initialize odometry publisher and messasge + // initialize odometry publisher and message odometry_publisher_ = get_node()->create_publisher( DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_publisher_ = diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 9720e068e1..755259cc2a 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -19,11 +19,17 @@ diff_drive_controller: type: double, default_value: 0.0, description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + validation: { + gt<>: [0.0] + } } wheel_radius: { type: double, default_value: 0.0, description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + validation: { + gt<>: [0.0] + } } wheel_separation_multiplier: { type: double, 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 bfbf8f2d19..606bacbf4f 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -4,7 +4,6 @@ test_diff_drive_controller: right_wheel_names: ["right_wheels"] wheel_separation: 0.40 - wheels_per_side: 1 # actually 2, but both are controlled by 1 signal wheel_radius: 0.02 wheel_separation_multiplier: 1.0 diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 9ab3022a9f..72ae4dbfd7 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -14,7 +14,6 @@ #include -#include #include #include #include @@ -26,7 +25,8 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::HW_IF_POSITION; @@ -57,22 +57,17 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr * @brief wait_for_twist block until a new twist is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function - * - * @return true if new twist msg was received, false if timeout */ - bool wait_for_twist( + void wait_for_twist( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) { - rclcpp::WaitSet wait_set; - wait_set.add_subscription(velocity_command_subscriber_); - - if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); - return true; + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return false; } /** @@ -184,6 +179,10 @@ class TestDiffDriveController : public ::testing::Test rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); parameter_overrides.push_back( rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); + // default parameters + parameter_overrides.push_back( + rclcpp::Parameter("wheel_separation", rclcpp::ParameterValue(1.0))); + parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1))); parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); @@ -546,7 +545,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) const double angular = 0.0; publish(linear, angular); // wait for msg is be published to the system - ASSERT_TRUE(controller_->wait_for_twist(executor)); + controller_->wait_for_twist(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 4c9d2f984f..302bdf6924 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -28,12 +28,15 @@ TEST(TestLoadDiffDriveController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique(ros2_control_test_assets::diffbot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::diffbot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/config/test_diff_drive_controller.yaml"; - ASSERT_NE( - cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), - nullptr); + cm.set_parameter({"test_diff_drive_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_diff_drive_controller.type", "diff_drive_controller/DiffDriveController"}); + + ASSERT_NE(cm.load_controller("test_diff_drive_controller"), nullptr); } int main(int argc, char ** argv) @@ -42,6 +45,5 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); int result = RUN_ALL_TESTS(); rclcpp::shutdown(); - rclcpp::shutdown(); return result; } diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index d042fe79dd..d5b956aa8a 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -6,7 +6,7 @@ ros2_controllers ################# -Commonly used and generalized controllers for ros2_control framework that are ready to use with many robots, `MoveIt2 `_ and `Nav2 `_. +Commonly used and generalized controllers for ros2_control framework that are ready to use with many robots, `MoveIt2 `_ and `Nav2 `_. `Link to GitHub Repository `_ @@ -16,23 +16,20 @@ Guidelines and Best Practices .. toctree:: :titlesonly: - :glob: - * + mobile_robot_kinematics.rst + writing_new_controller.rst -Controllers for Mobile Robots -***************************** +Controllers for Wheeled Mobile Robots +************************************* .. toctree:: :titlesonly: - Ackermann Steering Controller <../ackermann_steering_controller/doc/userdoc.rst> - Bicycle Steering Controller <../bicycle_steering_controller/doc/userdoc.rst> Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> Steering Controllers Library <../steering_controllers_library/doc/userdoc.rst> Tricycle Controller <../tricycle_controller/doc/userdoc.rst> - Tricycle Steering Controller <../tricycle_steering_controller/doc/userdoc.rst> Controllers for Manipulators and Other Robots ********************************************* @@ -55,6 +52,7 @@ The controllers are using `common hardware interface definitions`_, and may use Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> + Parallel Gripper Controller <../parallel_gripper_controller/doc/userdoc.rst> PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> @@ -73,6 +71,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> + Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst> Common Controller Parameters diff --git a/doc/images/.gitignore b/doc/images/.gitignore new file mode 100644 index 0000000000..8d71bf9cf4 --- /dev/null +++ b/doc/images/.gitignore @@ -0,0 +1 @@ +*.bkp diff --git a/doc/images/ackermann_steering.drawio b/doc/images/ackermann_steering.drawio new file mode 100644 index 0000000000..d0ddb06a56 --- /dev/null +++ b/doc/images/ackermann_steering.drawio @@ -0,0 +1,251 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/ackermann_steering.svg b/doc/images/ackermann_steering.svg new file mode 100644 index 0000000000..1ca6ace166 --- /dev/null +++ b/doc/images/ackermann_steering.svg @@ -0,0 +1,3 @@ + + +
ICR
diff --git a/doc/images/ackermann_steering_traction.drawio b/doc/images/ackermann_steering_traction.drawio new file mode 100644 index 0000000000..ccf40be4d7 --- /dev/null +++ b/doc/images/ackermann_steering_traction.drawio @@ -0,0 +1,287 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/ackermann_steering_traction.svg b/doc/images/ackermann_steering_traction.svg new file mode 100644 index 0000000000..1ee9d0181b --- /dev/null +++ b/doc/images/ackermann_steering_traction.svg @@ -0,0 +1,3 @@ + + +
ICR
diff --git a/doc/images/car_like_robot.drawio b/doc/images/car_like_robot.drawio new file mode 100644 index 0000000000..f9b87b9794 --- /dev/null +++ b/doc/images/car_like_robot.drawio @@ -0,0 +1,173 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/car_like_robot.svg b/doc/images/car_like_robot.svg new file mode 100644 index 0000000000..6fb6bf17d0 --- /dev/null +++ b/doc/images/car_like_robot.svg @@ -0,0 +1,3 @@ + + +
ICR
Front wheel
Rear wheel
diff --git a/doc/images/diff_drive.drawio b/doc/images/diff_drive.drawio new file mode 100644 index 0000000000..f5fc9802e0 --- /dev/null +++ b/doc/images/diff_drive.drawio @@ -0,0 +1,137 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/diff_drive.svg b/doc/images/diff_drive.svg new file mode 100644 index 0000000000..9759b86048 --- /dev/null +++ b/doc/images/diff_drive.svg @@ -0,0 +1,3 @@ + + +
diff --git a/doc/images/double_traction.drawio b/doc/images/double_traction.drawio new file mode 100644 index 0000000000..7f9989d733 --- /dev/null +++ b/doc/images/double_traction.drawio @@ -0,0 +1,197 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/double_traction.svg b/doc/images/double_traction.svg new file mode 100644 index 0000000000..ec60b11ffa --- /dev/null +++ b/doc/images/double_traction.svg @@ -0,0 +1,3 @@ + + +
ICR
diff --git a/doc/images/unicycle.drawio b/doc/images/unicycle.drawio new file mode 100644 index 0000000000..49a1f44b5a --- /dev/null +++ b/doc/images/unicycle.drawio @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/unicycle.svg b/doc/images/unicycle.svg new file mode 100644 index 0000000000..826bbb1463 --- /dev/null +++ b/doc/images/unicycle.svg @@ -0,0 +1,3 @@ + + +
diff --git a/doc/migration.rst b/doc/migration.rst new file mode 100644 index 0000000000..4c0e4608d6 --- /dev/null +++ b/doc/migration.rst @@ -0,0 +1,21 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration.rst + +Migration Guides: Iron to Jazzy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This list summarizes important changes between Iron (previous) and Jazzy (current) releases, where changes to user code might be necessary. + + +diff_drive_controller +***************************** +* The twist message on ``~/cmd_vel`` is now required to be of stamped type (`#812 `_). + +joint_trajectory_controller +***************************** + +* Parameter ``allow_nonzero_velocity_at_trajectory_end`` is now per default ``false`` (`#834 `_). +* The parameter ``start_with_holding`` is removed, it now always holds the position at activation (`#839 `_). +* Goals are now cancelled in ``on_deactivate`` transition (`#962 `_). +* Empty trajectory messages are discarded (`#902 `_). +* Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). +* The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint. +* Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 `_). Adaptions to the action goal might be necessary. diff --git a/doc/mobile_robot_kinematics.rst b/doc/mobile_robot_kinematics.rst new file mode 100644 index 0000000000..2ab4f1c0ee --- /dev/null +++ b/doc/mobile_robot_kinematics.rst @@ -0,0 +1,322 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/mobile_robot_kinematics.rst + +.. _mobile_robot_kinematics: + +Wheeled Mobile Robot Kinematics +-------------------------------------------------------------- + +.. _siciliano: https://link.springer.com/book/10.1007/978-1-84628-642-1 +.. _modern_robotics: http://modernrobotics.org/ + +This page introduces the kinematics of different wheeled mobile robots. For further reference see `Siciliano et.al - Robotics: Modelling, Planning and Control `_ and `Kevin M. Lynch and Frank C. Park - Modern Robotics: Mechanics, Planning, And Control `_. + +Wheeled mobile robots can be classified in two categories: + +Omnidirectional robots + which can move instantaneously in any direction in the plane, and + +Nonholonomic robots + which cannot move instantaneously in any direction in the plane. + +The forward integration of the kinematic model using the encoders of the wheel actuators — is referred to as **odometric localization** or **passive localization** or **dead reckoning**. We will call it just **odometry**. + +Omnidirectional Wheeled Mobile Robots +..................................... + +Robots with omniwheels or mecanum wheels. Section will be updated if controllers for these robots are implemented. + +Nonholonomic Wheeled Mobile Robots +..................................... + +Unicycle model +,,,,,,,,,,,,,,,, + +To define the coordinate systems (`ROS coordinate frame conventions `__, the coordinate systems follow the right-hand rule), consider the following simple unicycle model + +.. image:: images/unicycle.svg + :width: 550 + :align: center + :alt: Unicycle + +* :math:`x_b,y_b` is the robot's body-frame coordinate system, located at the contact point of the wheel on the ground. +* :math:`x_w,y_w` is the world coordinate system. +* :math:`x,y` are the robot's Cartesian coordinates in the world coordinate system. +* :math:`\theta` is the robot's heading angle, i.e. the orientation of the robot's :math:`x_b`-axis w.r.t. the world's :math:`x_w`-axis. + +In the following, we want to command the robot with a desired body twist + +.. math:: + + \vec{\nu}_b = \begin{bmatrix} + \vec{\omega}_{b} \\ + \vec{v}_{b} + \end{bmatrix}, + +where :math:`\vec{v}_{b}` is the linear velocity of the robot in its body-frame, and :math:`\vec\omega_{b}` is the angular velocity of the robot in its body-frame. As we consider steering robots on a flat surface, it is sufficient to give + +* :math:`v_{b,x}`, i.e. the linear velocity of the robot in direction of the :math:`x_b` axis. +* :math:`\omega_{b,z}`, i.e. the angular velocity of the robot about the :math:`x_z` axis. + +as desired system inputs. The forward kinematics of the unicycle can be calculated with + +.. math:: + \dot{x} &= v_{b,x} \cos(\theta) \\ + \dot{y} &= v_{b,x} \sin(\theta) \\ + \dot{\theta} &= \omega_{b,z} + +We will formulate the inverse kinematics to calculate the desired commands for the robot (wheel speed or steering) from the given body twist. + +Differential Drive Robot +,,,,,,,,,,,,,,,,,,,,,,,, + +Citing `Siciliano et.al - Robotics: Modelling, Planning and Control `_: + +.. code-block:: text + + A unicycle in the strict sense (i.e., a vehicle equipped with a single wheel) + is a robot with a serious problem of balance in static conditions. However, + there exist vehicles that are kinematically equivalent to a unicycle but more + stable from a mechanical viewpoint. + +One of these vehicles is the differential drive robot, which has two wheels, each of which is driven independently. + +.. image:: images/diff_drive.svg + :width: 550 + :align: center + :alt: Differential drive robot + +* :math:`w` is the wheel track (the distance between the wheels). + +**Forward Kinematics** + +The forward kinematics of the differential drive model can be calculated from the unicycle model above using + +.. math:: + v_{b,x} &= \frac{v_{right} + v_{left}}{2} \\ + \omega_{b,z} &= \frac{v_{right} - v_{left}}{w} + +**Inverse Kinematics** + +The necessary wheel speeds to achieve a desired body twist can be calculated with: + +.. math:: + + v_{left} &= v_{b,x} - \omega_{b,z} w / 2 \\ + v_{right} &= v_{b,x} + \omega_{b,z} w / 2 + + +**Odometry** + +We can use the forward kinematics equations above to calculate the robot's odometry directly from the encoder readings. + +Car-Like (Bicycle) Model +,,,,,,,,,,,,,,,,,,,,,,,, + +The following picture shows a car-like robot with two wheels, where the front wheel is steerable. This model is also known as the bicycle model. + +.. image:: images/car_like_robot.svg + :width: 550 + :align: center + :alt: Car-like robot + +* :math:`\phi` is the steering angle of the front wheel, counted positive in direction of rotation around :math:`x_z`-axis. +* :math:`v_{rear}, v_{front}` is the velocity of the rear and front wheel. +* :math:`l` is the wheelbase. + +We assume that the wheels are rolling without slipping. This means that the velocity of the contact point of the wheel with the ground is zero and the wheel's velocity points in the direction perpendicular to the wheel's axis. The **Instantaneous Center of Rotation** (ICR), i.e. the center of the circle around which the robot rotates, is located at the intersection of the lines that are perpendicular to the wheels' axes and pass through the contact points of the wheels with the ground. + +As a consequence of the no-slip condition, the velocity of the two wheels must satisfy the following constraint: + +.. math:: + v_{rear} = v_{front} \cos(\phi) + +**Forward Kinematics** + +The forward kinematics of the car-like model can be calculated with + +.. math:: + \dot{x} &= v_{b,x} \cos(\theta) \\ + \dot{y} &= v_{b,x} \sin(\theta) \\ + \dot{\theta} &= \frac{v_{b,x}}{l} \tan(\phi) + + +**Inverse Kinematics** + +The steering angle is one command input of the robot: + +.. math:: + \phi = \arctan\left(\frac{l w_{b,z}}{v_{b,x}} \right) + + +For the rear-wheel drive, the velocity of the rear wheel is the second input of the robot: + +.. math:: + v_{rear} = v_{b,x} + + +For the front-wheel drive, the velocity of the front wheel is the second input of the robot: + +.. math:: + v_{front} = \frac{v_{b,x}}{\cos(\phi)} + +**Odometry** + +We have to distinguish between two cases: Encoders on the rear wheel or on the front wheel. + +For the rear wheel case: + +.. math:: + \dot{x} &= v_{rear} \cos(\theta) \\ + \dot{y} &= v_{rear} \sin(\theta) \\ + \dot{\theta} &= \frac{v_{rear}}{l} \tan(\phi) + + +For the front wheel case: + +.. math:: + \dot{x} &= v_{front} \cos(\theta) \cos(\phi)\\ + \dot{y} &= v_{front} \sin(\theta) \cos(\phi)\\ + \dot{\theta} &= \frac{v_{front}}{l} \sin(\phi) + + +Double-Traction Axle +,,,,,,,,,,,,,,,,,,,,, + +The following image shows a car-like robot with three wheels, with two independent traction wheels at the rear. + +.. image:: images/double_traction.svg + :width: 550 + :align: center + :alt: A car-like robot with two traction wheels at the rear + +* :math:`w_r` is the wheel track of the rear axle. + +**Forward Kinematics** + +The forward kinematics is the same as the car-like model above. + +**Inverse Kinematics** + +The turning radius of the robot is + +.. math:: + R_b = \frac{l}{\tan(\phi)} + +Then the velocity of the rear wheels must satisfy these conditions to avoid skidding + +.. math:: + v_{rear,left} &= v_{b,x}\frac{R_b - w_r/2}{R_b}\\ + v_{rear,right} &= v_{b,x}\frac{R_b + w_r/2}{R_b} + +**Odometry** + +The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is overdetermined. +If there is no slip and the encoders are ideal, + +.. math:: + v_{b,x} = v_{rear,left} \frac{R_b}{R_b - w_r/2} = v_{rear,right} \frac{R_b}{R_b + w_r/2} + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + v_{b,x} = 0.5 \left(v_{rear,left} \frac{R_b}{R_b - w_r/2} + v_{rear,right} \frac{R_b}{R_b + w_r/2}\right). + + +Ackermann Steering +,,,,,,,,,,,,,,,,,,,,, + +The following image shows a four-wheeled robot with two independent steering wheels in the front. + +.. image:: images/ackermann_steering.svg + :width: 550 + :align: center + :alt: A car-like robot with two steering wheels at the front + +* :math:`w_f` is the wheel track of the front axle, measured between the two kingpins. + +To prevent the front wheels from slipping, the steering angle of the front wheels cannot be equal. +This is the so-called **Ackermann steering**. + +.. note:: + Ackermann steering can also be achieved by a `mechanical linkage between the two front wheels `__. In this case the robot has only one steering input, and the steering angle of the two front wheels is mechanically coupled. The inverse kinematics of the robot will then be the same as in the car-like model above. + +**Forward Kinematics** + +The forward kinematics is the same as for the car-like model above. + +**Inverse Kinematics** + +The turning radius of the robot is + +.. math:: + R_b = \frac{l}{\tan(\phi)} + +Then the steering angles of the front wheels must satisfy these conditions to avoid skidding + +.. math:: + \phi_{left} &= \arctan\left(\frac{l}{R_b - w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) - w_f\sin(\phi)}\right)\\ + \phi_{right} &= \arctan\left(\frac{l}{R_b + w_f/2}\right) &= \arctan\left(\frac{2l\sin(\phi)}{2l\cos(\phi) + w_f\sin(\phi)}\right) + +**Odometry** + +The calculation of :math:`\phi` from two angle measurements of the steering axle is overdetermined. +If there is no slip and the measurements are ideal, + +.. math:: + \phi = \arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) = \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right) + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + \phi = 0.5 \left(\arctan\left(\frac{l\tan(\phi_{left})}{l + w_f/2 \tan(\phi_{left})}\right) + \arctan\left(\frac{l\tan(\phi_{right})}{l - w_f/2 \tan(\phi_{right})}\right)\right). + +Ackermann Steering with Traction +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +The following image shows a four-wheeled car-like robot with two independent steering wheels at the front, which are also driven independently. + +.. image:: images/ackermann_steering_traction.svg + :width: 550 + :align: center + :alt: A car-like robot with two steering wheels at the front, which are also independently driven. + +* :math:`d_{kp}` is the distance from the kingpin to the contact point of the front wheel with the ground. + +**Forward Kinematics** + +The forward kinematics is the same as the car-like model above. + +**Inverse Kinematics** + +To avoid slipping of the front wheels, the velocity of the front wheels cannot be equal and + +.. math:: + \frac{v_{front,left}}{R_{left}} = \frac{v_{front,right}}{R_{right}} = \frac{v_{b,x}}{R_b} + +with turning radius of the robot and the left/right front wheel + +.. math:: + R_b &= \frac{l}{\tan(\phi)} \\ + R_{left} &= \frac{l-d_{kp}\sin(\phi_{left})}{\sin(\phi_{left})}\\ + R_{right} &= \frac{l+d_{kp}\sin(\phi_{right})}{\sin(\phi_{right})}. + +This results in the following inverse kinematics equations + +.. math:: + v_{front,left} &= \frac{v_{b,x}(l-d_{kp}\sin(\phi_{left}))}{R_b\sin(\phi_{left})}\\ + v_{front,right} &= \frac{v_{b,x}(l+d_{kp}\sin(\phi_{right}))}{R_b\sin(\phi_{right})} + +with the steering angles of the front wheels from the Ackermann steering equations above. + +**Odometry** + +The calculation of :math:`v_{b,x}` from two encoder measurements of the traction axle is again overdetermined. +If there is no slip and the encoders are ideal, + +.. math:: + v_{b,x} = v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} = v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})} + +holds. But to get a more robust solution, we take the average of both , i.e., + +.. math:: + v_{b,x} = 0.5 \left( v_{front,left} \frac{R_b\sin(\phi_{left})}{l-d_{kp}\sin(\phi_{left})} + v_{front,right} \frac{R_b\sin(\phi_{right})}{l+d_{kp}\sin(\phi_{right})}\right). diff --git a/doc/release_notes.rst b/doc/release_notes.rst new file mode 100644 index 0000000000..688c35724d --- /dev/null +++ b/doc/release_notes.rst @@ -0,0 +1,65 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes.rst + +Release Notes: Iron to Jazzy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This list summarizes the changes between Iron (previous) and Jazzy (current) releases. + +admittance_controller +************************ +* Remove ``robot_description`` parameter from parameter YAML, because it is not used at all (`#963 `_). + +diff_drive_controller +***************************** +* The twist message on ``~/cmd_vel`` is now required to be of stamped type (`#812 `_). +* Remove unused parameter ``wheels_per_side`` (`#958 `_). + +joint_trajectory_controller +***************************** + +* Parameter ``allow_nonzero_velocity_at_trajectory_end`` is now per default ``false`` (`#834 `_). +* Activate update of dynamic parameters (`#761 `_ and `#849 `_). +* The parameter ``start_with_holding`` is removed, it now always holds the position at activation (`#839 `_). +* Continue with last trajectory-point on success, instead of hold-position from current state (`#842 `_). +* Add console output for tolerance checks (`#932 `_): + + .. code:: + + [tolerances]: State tolerances failed for joint 2: + [tolerances]: Position Error: 0.020046, Position Tolerance: 0.010000 + [trajectory_controllers]: Aborted due goal_time_tolerance exceeding by 1.010000 seconds + +* Goals are now cancelled in ``on_deactivate`` transition (`#962 `_). +* Empty trajectory messages are discarded (`#902 `_). +* Action field ``error_string`` is now filled with meaningful strings (`#887 `_). +* Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). +* The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). ``angle_wraparound`` parameter was completely removed. +* Tolerances sent with the action goal are now processed and used for the action. (`#716 `_). For details, see the `JointTolerance message `_: + + .. code-block:: markdown + + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. + + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. + +* Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 `_). + +pid_controller +************************ +* 🚀 The PID controller was added 🎉 (`#434 `_). + +steering_controllers_library +******************************** +* Changing default int values to double in steering controller's yaml file. The controllers should now initialize successfully without specifying these parameters (`#927 `_). +* A fix for Ackermann steering odometry was added (`#921 `_). +* Do not reset the steering wheels to ``0.0`` on timeout (`#1289 `_). + +tricycle_controller +************************ +* tricycle_controller now uses generate_parameter_library (`#957 `_). diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 1a9ffce714..5aa7068b98 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -42,7 +42,7 @@ 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) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``get_node_options``. + 6. (Optional) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``define_custom_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. diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index ac47c3c2b3..ea2efdcf4e 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,42 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp index 562bbea52f..b07e8a630c 100644 --- a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp +++ b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp @@ -15,11 +15,8 @@ #ifndef EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ #define EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ -#include - #include "effort_controllers/visibility_control.h" #include "forward_command_controller/forward_command_controller.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace effort_controllers { diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 52165f6ec1..fab6d671ad 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.6.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp index c3748d493c..e7d0f274a5 100644 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ b/effort_controllers/src/joint_group_effort_controller.cpp @@ -17,7 +17,6 @@ #include "controller_interface/controller_interface.hpp" #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" namespace effort_controllers diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index 200a1beda8..c19cbff9cf 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -12,35 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_effort_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupEffortControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupEffortControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +47,7 @@ void JointGroupEffortControllerTest::SetUpController() command_ifs.emplace_back(joint_2_cmd_); command_ifs.emplace_back(joint_3_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupEffortControllerTest, JointsParameterNotSet) @@ -204,10 +189,13 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index 6ae9db4670..5af880d792 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -15,15 +15,16 @@ #ifndef TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; @@ -57,6 +58,7 @@ class JointGroupEffortControllerTest : public ::testing::Test CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]}; CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]}; CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp index 52f1f9934a..0ac1053d18 100644 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_load_joint_group_effort_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index c61f166b8e..7d2a7a1a8a 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,46 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 38c984192e..02c343f050 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -53,9 +53,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_force_torque_sensor_broadcaster - test/test_load_force_torque_sensor_broadcaster.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/force_torque_sensor_broadcaster_params.yaml) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_force_torque_sensor_broadcaster test/test_load_force_torque_sensor_broadcaster.cpp) target_include_directories(test_load_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_force_torque_sensor_broadcaster force_torque_sensor_broadcaster diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 754d1de9ba..bd477ed68a 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -20,15 +20,12 @@ #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #include -#include -#include #include "controller_interface/controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/force_torque_sensor.hpp" diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 178a90d5cf..1383342246 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.6.0 + 4.15.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl 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 2412361352..1ea25520cc 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 @@ -22,14 +22,11 @@ #include #include -#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedStateInterface; using testing::IsEmpty; @@ -75,31 +72,40 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message( geometry_msgs::msg::WrenchStamped & wrench_msg) { // create a new subscriber + geometry_msgs::msg::WrenchStamped::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr) {}; + auto subs_callback = [&](const geometry_msgs::msg::WrenchStamped::SharedPtr msg) + { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(wrench_msg, msg_info)); + wrench_msg = *received_msg; } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index fe5b0ab3ba..fe7fb0c174 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -19,11 +19,11 @@ #ifndef TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #define TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ +#include + #include #include -#include "gmock/gmock.h" - #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" // subclassing and friending so we can access member variables diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index 0c269d6a31..b5887f09e1 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -32,15 +32,17 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); - - ASSERT_NE( - cm.load_controller( - "test_force_torque_sensor_broadcaster", - "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"), - nullptr); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/force_torque_sensor_broadcaster_params.yaml"; + + cm.set_parameter({"test_force_torque_sensor_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_force_torque_sensor_broadcaster.type", + "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_force_torque_sensor_broadcaster"), nullptr); } int main(int argc, char ** argv) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 54a428ec3f..54caa9d764 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,42 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index 91e3aae480..12f2a28b22 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -16,8 +16,6 @@ #define FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ #include -#include -#include #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index d60245f328..4858e5ab64 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -22,7 +22,6 @@ #include "controller_interface/controller_interface.hpp" #include "forward_command_controller/visibility_control.h" #include "rclcpp/subscription.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "std_msgs/msg/float64_multi_array.hpp" diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index fd7c0d480e..50476c62e3 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -16,8 +16,6 @@ #define FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ #include -#include -#include #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 9b6c2559f4..f9f30dd053 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.6.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 0305a37e4e..78fe8c9191 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -14,14 +14,11 @@ #include "forward_command_controller/forward_command_controller.hpp" -#include #include #include -#include #include #include "rclcpp/logging.hpp" -#include "rclcpp/qos.hpp" namespace forward_command_controller { diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index e4ea46fcc5..331d3f9eea 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -14,10 +14,8 @@ #include "forward_command_controller/forward_controllers_base.hpp" -#include #include #include -#include #include #include "controller_interface/helpers.hpp" diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index c31c8a964c..fcfa65ee1c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -30,24 +30,12 @@ #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; using testing::IsEmpty; using testing::SizeIs; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void ForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void ForwardCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -71,6 +59,7 @@ void ForwardCommandControllerTest::SetUpController() command_ifs.emplace_back(joint_2_pos_cmd_); command_ifs.emplace_back(joint_3_pos_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(ForwardCommandControllerTest, JointsParameterNotSet) @@ -320,10 +309,13 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 9c6bd2a352..2284d46d61 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -24,6 +24,7 @@ #include "forward_command_controller/forward_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; @@ -69,6 +70,7 @@ class ForwardCommandControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index b493e52b2a..c192f1eb5f 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadForwardCommandController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp index 41a9b74698..49bc277824 100644 --- a/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_multi_interface_forward_command_controller.cpp @@ -31,9 +31,7 @@ TEST(TestLoadMultiInterfaceForwardController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( 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 7879d5c1d7..c52f4f3ab6 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -32,24 +32,12 @@ #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; using testing::IsEmpty; using testing::SizeIs; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void MultiInterfaceForwardCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void MultiInterfaceForwardCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -74,6 +62,7 @@ void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params command_ifs.emplace_back(joint_1_vel_cmd_); command_ifs.emplace_back(joint_1_eff_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); if (set_params_and_activate) { @@ -273,10 +262,13 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 62a4d4e981..78b244847b 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -26,6 +26,7 @@ #include "forward_command_controller/multi_interface_forward_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; @@ -77,6 +78,7 @@ class MultiInterfaceForwardCommandControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &pos_cmd_}; CommandInterface joint_1_vel_cmd_{joint_name_, HW_IF_VELOCITY, &vel_cmd_}; CommandInterface joint_1_eff_cmd_{joint_name_, HW_IF_EFFORT, &eff_cmd_}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 76ea9418d2..11c61f86cf 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,44 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1199 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (`#1002 `_) +* Contributors: Henry Moore, Paul Gesel, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 77598591ae..2c115091af 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -40,6 +40,11 @@ void GripperActionController::preempt_active_goal() template controller_interface::CallbackReturn GripperActionController::on_init() { + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated]: the `position_controllers/GripperActionController` and " + "`effort_controllers::GripperActionController` controllers are replaced by " + "'parallel_gripper_controllers/GripperActionController' controller"); try { param_listener_ = std::make_shared(get_node()); diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index b125ab12d0..2cc24794b8 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include "control_toolbox/pid.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a6b83b43ec..d46c98f608 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.6.0 + 4.15.0 The gripper_controllers package Bence Magyar diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 9f7e024917..97fef24eaf 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include #include @@ -23,9 +22,6 @@ #include "test_gripper_controllers.hpp" #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 4983c8102d..cb92bc9dcd 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -15,12 +15,12 @@ #ifndef TEST_GRIPPER_CONTROLLERS_HPP_ #define TEST_GRIPPER_CONTROLLERS_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "gripper_controllers/gripper_action_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/gripper_controllers/test/test_load_gripper_action_controllers.cpp b/gripper_controllers/test/test_load_gripper_action_controllers.cpp index 0ef5f0bcb2..b607fb6fe7 100644 --- a/gripper_controllers/test/test_load_gripper_action_controllers.cpp +++ b/gripper_controllers/test/test_load_gripper_action_controllers.cpp @@ -30,9 +30,7 @@ TEST(TestLoadGripperActionControllers, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index d906fb897f..40af7a5360 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,46 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 6782012c65..5539dea9ff 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -53,9 +53,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_imu_sensor_broadcaster - test/test_load_imu_sensor_broadcaster.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/imu_sensor_broadcaster_params.yaml) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_imu_sensor_broadcaster test/test_load_imu_sensor_broadcaster.cpp) target_include_directories(test_load_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_imu_sensor_broadcaster imu_sensor_broadcaster diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index cd2a44d32b..1ba0b032ac 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -20,14 +20,11 @@ #define IMU_SENSOR_BROADCASTER__IMU_SENSOR_BROADCASTER_HPP_ #include -#include -#include #include "controller_interface/controller_interface.hpp" #include "imu_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "imu_sensor_broadcaster_parameters.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/imu_sensor.hpp" diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 5a4ca12e87..ed1f794afa 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.6.0 + 4.15.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 25a39a8b4d..67cf31f8a5 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -23,12 +23,9 @@ #include #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" -#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "sensor_msgs/msg/imu.hpp" using hardware_interface::LoanedStateInterface; @@ -77,31 +74,39 @@ void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg) { // create a new subscriber + sensor_msgs::msg::Imu::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::Imu::SharedPtr msg) { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_imu_sensor_broadcaster/imu", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(imu_msg, msg_info)); + imu_msg = *received_msg; } TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 01423724b8..0f3286c302 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -19,11 +19,11 @@ #ifndef TEST_IMU_SENSOR_BROADCASTER_HPP_ #define TEST_IMU_SENSOR_BROADCASTER_HPP_ +#include + #include #include -#include "gmock/gmock.h" - #include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" // subclassing and friending so we can access member variables diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index f4e6105ed6..eca29082c1 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -32,14 +32,15 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); - - ASSERT_NE( - cm.load_controller( - "test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster"), - nullptr); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/imu_sensor_broadcaster_params.yaml"; + + cm.set_parameter({"test_imu_sensor_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_imu_sensor_broadcaster.type", "imu_sensor_broadcaster/IMUSensorBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_imu_sensor_broadcaster"), nullptr); } int main(int argc, char ** argv) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 1444f5f8ab..cf422e3eb8 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,50 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* [JSB] Move the initialize of urdf::Model from on_activate to on_configure to improve real-time performance (`#1269 `_) +* Contributors: Takashi Sato + +4.13.0 (2024-08-22) +------------------- +* [Joint State Broadcaster] Publish the joint_states of joints present in the URDF (`#1233 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ +* added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Contributors: Sai Kishor Kothakota + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 5c383897cc..cc8dc18bf6 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rcutils realtime_tools sensor_msgs + urdf ) find_package(ament_cmake REQUIRED) @@ -72,6 +73,7 @@ if(BUILD_TESTING) ) ament_target_dependencies(test_joint_state_broadcaster hardware_interface + ros2_control_test_assets ) endif() diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index f1c532dce9..ecc3c767f6 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -25,10 +25,9 @@ #include "joint_state_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "joint_state_broadcaster_parameters.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "realtime_tools/realtime_publisher.h" #include "sensor_msgs/msg/joint_state.hpp" +#include "urdf/model.h" namespace joint_state_broadcaster { @@ -113,6 +112,9 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface dynamic_joint_state_publisher_; std::shared_ptr> realtime_dynamic_joint_state_publisher_; + + urdf::Model model_; + bool is_model_loaded_ = false; }; } // namespace joint_state_broadcaster diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 0bf3db58dd..c3a1e66574 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.6.0 + 4.15.0 Broadcaster to publish joint state Bence Magyar Denis Stogl @@ -22,6 +22,7 @@ rcutils realtime_tools sensor_msgs + urdf ament_cmake_gmock controller_manager diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 3c2192d40e..9bbb862925 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -14,23 +14,18 @@ #include "joint_state_broadcaster/joint_state_broadcaster.hpp" -#include +#include #include #include #include #include #include -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/clock.hpp" -#include "rclcpp/event_handler.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rcpputils/split.hpp" -#include "rcutils/logging_macros.h" #include "std_msgs/msg/header.hpp" +#include "urdf/model.h" namespace rclcpp_lifecycle { @@ -169,6 +164,18 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } + + const std::string & urdf = get_robot_description(); + + is_model_loaded_ = !urdf.empty() && model_.initString(urdf); + if (!is_model_loaded_) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'", + HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT); + } + return CallbackReturn::SUCCESS; } @@ -202,6 +209,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { joint_names_.clear(); + name_if_value_mapping_.clear(); return CallbackReturn::SUCCESS; } @@ -255,7 +263,12 @@ bool JointStateBroadcaster::init_joint_data() const auto & interfaces_and_values = name_ifv.second; if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT})) { - joint_names_.push_back(name_ifv.first); + if ( + !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded_ || + model_.getJoint(name_ifv.first)) + { + joint_names_.push_back(name_ifv.first); + } } } @@ -297,6 +310,8 @@ void JointStateBroadcaster::init_joint_state_msg() void JointStateBroadcaster::init_dynamic_joint_state_msg() { auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_; + dynamic_joint_state_msg.joint_names.clear(); + dynamic_joint_state_msg.interface_values.clear(); for (const auto & name_ifv : name_if_value_mapping_) { const auto & name = name_ifv.first; diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index 8f0d4da6c5..c8ca928a12 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -39,3 +39,10 @@ joint_state_broadcaster: type: string, default_value: "effort", } + use_urdf_to_filter: { + type: bool, + default_value: true, + description: "Uses the robot_description to filter the ``joint_states`` topic. + If true, the broadcaster will publish the data of the joints present in the URDF alone. + If false, the broadcaster will publish the data of any interface that has type ``position``, ``velocity``, or ``effort``." + } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 2faa55f467..de534c00b6 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -23,12 +23,12 @@ #include "gmock/gmock.h" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "joint_state_broadcaster/joint_state_broadcaster.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" #include "test_joint_state_broadcaster.hpp" using hardware_interface::HW_IF_EFFORT; @@ -62,15 +62,17 @@ void JointStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); void JointStateBroadcasterTest::SetUpStateBroadcaster( const std::vector & joint_names, const std::vector & interfaces) { - init_broadcaster_and_set_parameters(joint_names, interfaces); + init_broadcaster_and_set_parameters("", joint_names, interfaces); assign_state_interfaces(joint_names, interfaces); } void JointStateBroadcasterTest::init_broadcaster_and_set_parameters( - const std::vector & joint_names, const std::vector & interfaces) + const std::string & robot_description, const std::vector & joint_names, + const std::vector & interfaces) { const auto result = state_broadcaster_->init( - "joint_state_broadcaster", "", 0, "", state_broadcaster_->define_custom_node_options()); + "joint_state_broadcaster", robot_description, 0, "", + state_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); state_broadcaster_->get_node()->set_parameter({"joints", joint_names}); @@ -214,6 +216,102 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) ElementsAreArray(interface_names_)); } +TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest) +{ + // publishers not initialized yet + ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); + ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_); + + SetUpStateBroadcaster(); + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_names_.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); + + // Now deactivate and activate with only 2 set of joints and interfaces (to create as in one of + // the interface is unavailable) + ASSERT_EQ(state_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + const std::vector JOINT_NAMES = {"joint1", "joint2"}; + assign_state_interfaces(JOINT_NAMES, interface_names_); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS_WITH_ONE_DEACTIVATED = JOINT_NAMES.size(); + + // check interface configuration + cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & new_joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(new_joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(new_joint_state_msg.position, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_joint_state_msg.velocity, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_joint_state_msg.effort, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + + // dynamic joint state initialized + const auto & new_dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); +} + TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) { const std::vector JOINT_NAMES = {}; @@ -263,6 +361,212 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) ElementsAreArray(interface_names_)); } +TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF) +{ + const std::vector JOINT_NAMES = {}; + const std::vector IF_NAMES = {interface_names_[0]}; + init_broadcaster_and_set_parameters("", JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_names_.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription) +{ + const std::vector JOINT_NAMES = {}; + const std::vector IF_NAMES = {interface_names_[0]}; + + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail; + const std::vector joint_in_urdf({"joint1", "joint2"}); + init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_in_urdf.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_in_urdf)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized and it will have the data of all the interfaces + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size())); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size())); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces) +{ + const std::vector JOINT_NAMES = {"joint1"}; + const std::vector IF_NAMES = {}; + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail; + const std::vector joint_in_urdf({"joint1", "joint2"}); + init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_in_urdf.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_in_urdf)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized and it will have the data of all the interfaces + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size())); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size())); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndInterfaces) +{ + const std::vector JOINT_NAMES = {"joint1"}; + const std::vector IF_NAMES = interface_names_; + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail; + init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = JOINT_NAMES.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized and it will have the data of all the interfaces + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); +} + TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) { const std::vector JOINT_NAMES = {"joint1"}; @@ -427,7 +731,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing) const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector IF_NAMES = {interface_names_[0], interface_names_[1]}; - init_broadcaster_and_set_parameters(JOINT_NAMES, {interface_names_[0], interface_names_[1]}); + init_broadcaster_and_set_parameters("", JOINT_NAMES, {interface_names_[0], interface_names_[1]}); // assign state with interfaces which are not set in parameters --> We should actually not assign // anything because CM will also not do that @@ -445,7 +749,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector IF_NAMES = {interface_names_[0], interface_names_[1]}; - init_broadcaster_and_set_parameters(JOINT_NAMES, {interface_names_[0], interface_names_[1]}); + init_broadcaster_and_set_parameters("", JOINT_NAMES, {interface_names_[0], interface_names_[1]}); // Manually assign existing interfaces --> one we need is missing std::vector state_ifs; @@ -664,31 +968,41 @@ void JointStateBroadcasterTest::activate_and_get_joint_state_message( node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + sensor_msgs::msg::JointState::SharedPtr received_msg; rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr msg) + { received_msg = msg; }; auto subscription = test_node.create_subscription(topic, 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node.get_node_base_interface()); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_node.get_clock()->now() + timeout; + while (!received_msg && test_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(joint_state_msg, msg_info)); + joint_state_msg = *received_msg; } void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) @@ -731,51 +1045,58 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( node_state = state_broadcaster_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + control_msgs::msg::DynamicJointState::SharedPtr dynamic_joint_state_msg; rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr) {}; + auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr msg) + { dynamic_joint_state_msg = msg; }; auto subscription = test_node.create_subscription(topic, 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node.get_node_base_interface()); + dynamic_joint_state_msg.reset(); + ASSERT_FALSE(dynamic_joint_state_msg); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_node.get_clock()->now() + timeout; + while (test_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (dynamic_joint_state_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; - - // take message from subscription - control_msgs::msg::DynamicJointState dynamic_joint_state_msg; - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(dynamic_joint_state_msg, msg_info)); + ASSERT_TRUE(dynamic_joint_state_msg); const size_t NUM_JOINTS = 3; const std::vector INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; - ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg->joint_names, SizeIs(NUM_JOINTS)); // the order in the message may be different // we only check that all values in this test are present in the message // and that it is the same across the interfaces // for test purposes they are mapped to the same doubles - for (size_t i = 0; i < dynamic_joint_state_msg.joint_names.size(); ++i) + for (size_t i = 0; i < dynamic_joint_state_msg->joint_names.size(); ++i) { ASSERT_THAT( - dynamic_joint_state_msg.interface_values[i].interface_names, + dynamic_joint_state_msg->interface_values[i].interface_names, ElementsAreArray(INTERFACE_NAMES)); const auto index_in_original_order = std::distance( joint_names_.cbegin(), std::find( - joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg.joint_names[i])); + joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i])); ASSERT_THAT( - dynamic_joint_state_msg.interface_values[i].values, + dynamic_joint_state_msg->interface_values[i].values, Each(joint_values_[index_in_original_order])); } } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index fa9d29c936..4f4241d3d7 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -15,12 +15,12 @@ #ifndef TEST_JOINT_STATE_BROADCASTER_HPP_ #define TEST_JOINT_STATE_BROADCASTER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "joint_state_broadcaster/joint_state_broadcaster.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -34,7 +34,12 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr { FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest); + FRIEND_TEST(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithJointsAndInterfaces); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter); FRIEND_TEST(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces); @@ -60,8 +65,8 @@ class JointStateBroadcasterTest : public ::testing::Test const std::vector & interfaces = {}); void init_broadcaster_and_set_parameters( - const std::vector & joint_names = {}, - const std::vector & interfaces = {}); + const std::string & robot_description, const std::vector & joint_names, + const std::vector & interfaces); void assign_state_interfaces( const std::vector & joint_names = {}, diff --git a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp index 5efb587805..b3cf778063 100644 --- a/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_load_joint_state_broadcaster.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointStateBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 17b26ad551..81696246b1 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,60 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* rename get/set_state to get/set_lifecylce_state (`#1250 `_) +* Contributors: Manuel Muth + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* [JTC] Refactor URDF Model parsing (`#1227 `_) +* Use the internal methods instead of using the variables directly (`#1221 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* [JTC] Fix test_tolerances_via_actions (`#1209 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (`#1192 `_) +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* [JTC] Process tolerances sent with action goal (`#716 `_) +* Contributors: Christoph Fröhlich, Felix Exner (fexner), Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- +* Remove manual angle-wraparound parameter (`#1152 `_) +* Contributors: Christoph Fröhlich + +4.9.0 (2024-06-05) +------------------ +* JTC trajectory end time validation fix (`#1090 `_) +* Contributors: Henry Moore + +4.8.0 (2024-05-14) +------------------ +* [JTC] Remove unused test code (`#1095 `_) +* Contributors: Bence Magyar + +4.7.0 (2024-03-22) +------------------ +* Remove action_msg dependency (`#1077 `_) +* Bump version of pre-commit hooks (`#1073 `_) +* Added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Parse URDF for continuous joints (`#949 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, github-actions[bot] + 4.6.0 (2024-02-12) ------------------ * Fix usage of M_PI on Windows (`#1036 `_) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index c92d61b3f3..438661f7bf 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rsl tl_expected trajectory_msgs + urdf ) find_package(ament_cmake REQUIRED) @@ -58,14 +59,21 @@ if(BUILD_TESTING) ament_add_gmock(test_trajectory test/test_trajectory.cpp) target_link_libraries(test_trajectory joint_trajectory_controller) + ament_target_dependencies(test_trajectory ros2_control_test_assets) target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES) + ament_add_gmock(test_tolerances test/test_tolerances.cpp) + target_link_libraries(test_tolerances joint_trajectory_controller) + ament_target_dependencies(test_tolerances ros2_control_test_assets) + target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES) + ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) target_link_libraries(test_trajectory_controller joint_trajectory_controller ) + ament_target_dependencies(test_trajectory_controller ros2_control_test_assets) target_compile_definitions(joint_trajectory_controller PRIVATE _USE_MATH_DEFINES) ament_add_gmock(test_load_joint_trajectory_controller @@ -88,6 +96,7 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory_actions joint_trajectory_controller ) + ament_target_dependencies(test_trajectory_actions ros2_control_test_assets) endif() diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml index 2ffe8aed36..4e4dacf202 100644 --- a/joint_trajectory_controller/doc/parameters_context.yaml +++ b/joint_trajectory_controller/doc/parameters_context.yaml @@ -9,5 +9,10 @@ gains: | .. 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), + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. + + If the joint is of continuous type, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`, + i.e., the shortest rotation to the target position is the desired motion. + Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured + position :math:`s` from the state interface. diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index af495ad14d..4dcb71a064 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -152,7 +152,21 @@ Actions [#f1]_ The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. -Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. +Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. For details, see the `JointTolerance message `_: + +.. code-block:: markdown + + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. + + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. + When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held. 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 21d83b4dde..1d157096e4 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -15,7 +15,6 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ -#include #include // for std::reference_wrapper #include #include @@ -36,7 +35,6 @@ #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" #include "rclcpp_action/server.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" @@ -246,7 +244,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa size_t joint_names_size, const std::vector & vector_field, const std::string & string_for_vector_field, size_t i, bool allow_empty) const; + // the tolerances from the node parameter SegmentTolerances default_tolerances_; + // the tolerances used for the current goal + realtime_tools::RealtimeBuffer active_tolerances_; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index c46b1c297f..4902fd1dcc 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -30,13 +30,14 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ +#include +#include +#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" #include "joint_trajectory_controller_parameters.hpp" -#include "rclcpp/node.hpp" - namespace joint_trajectory_controller { /** @@ -85,16 +86,19 @@ struct SegmentTolerances * goal: 0.01 * \endcode * + * \param jtc_logger The logger to use for output * \param params The ROS Parameters * \return Trajectory segment tolerances. */ -SegmentTolerances get_segment_tolerances(Params const & params) +SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params) { auto const & constraints = params.constraints; auto const n_joints = params.joints.size(); SegmentTolerances tolerances; tolerances.goal_time_tolerance = constraints.goal_time; + static auto logger = jtc_logger.get_child("tolerance"); + RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time); // State and goal state tolerances tolerances.state_tolerance.resize(n_joints); @@ -106,16 +110,178 @@ SegmentTolerances get_segment_tolerances(Params const & params) tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal; tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; - auto logger = rclcpp::get_logger("tolerance"); RCLCPP_DEBUG( - logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); + logger, "%s %f", (joint + ".trajectory.position").c_str(), + tolerances.state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal.position").c_str(), + tolerances.goal_state_tolerance[i].position); RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); + logger, "%s %f", (joint + ".goal.velocity").c_str(), + tolerances.goal_state_tolerance[i].velocity); } return tolerances; } +double resolve_tolerance_source(const double default_value, const double goal_value) +{ + // from + // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg + // There are two special values for tolerances: + // * 0 - The tolerance is unspecified and will remain at whatever the default is + // * -1 - The tolerance is "erased". + // If there was a default, the joint will be allowed to move without restriction. + constexpr double ERASE_VALUE = -1.0; + auto is_erase_value = [=](double value) + { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + + if (goal_value > 0.0) + { + return goal_value; + } + else if (is_erase_value(goal_value)) + { + return 0.0; + } + else if (goal_value < 0.0) + { + throw std::runtime_error("Illegal tolerance value."); + } + return default_value; +} + +/** + * \brief Populate trajectory segment tolerances using data from an action goal. + * + * \param jtc_logger The logger to use for output + * \param default_tolerances The default tolerances to use if the action goal does not specify any. + * \param goal The new action goal + * \param joints The joints configured by ROS parameters + * \return Trajectory segment tolerances. + */ +SegmentTolerances get_segment_tolerances( + rclcpp::Logger & jtc_logger, const SegmentTolerances & default_tolerances, + const control_msgs::action::FollowJointTrajectory::Goal & goal, + const std::vector & joints) +{ + SegmentTolerances active_tolerances(default_tolerances); + static auto logger = jtc_logger.get_child("tolerance"); + + try + { + active_tolerances.goal_time_tolerance = resolve_tolerance_source( + default_tolerances.goal_time_tolerance, rclcpp::Duration(goal.goal_time_tolerance).seconds()); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + logger, "Specified illegal goal_time_tolerance: " + << rclcpp::Duration(goal.goal_time_tolerance).seconds() + << ". Using default tolerances"); + return default_tolerances; + } + RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); + + // State and goal state tolerances + for (auto joint_tol : goal.path_tolerance) + { + auto const joint = joint_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance does not exist. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + auto i = std::distance(joints.cbegin(), it); + std::string interface = ""; + try + { + interface = "position"; + active_tolerances.state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.state_tolerance[i].position, joint_tol.position); + interface = "velocity"; + active_tolerances.state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.state_tolerance[i].velocity, joint_tol.velocity); + interface = "acceleration"; + active_tolerances.state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.state_tolerance[i].acceleration, joint_tol.acceleration); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + logger, "joint '" << joint << "' specified in goal.path_tolerance has a invalid " + << interface << " tolerance. Using default tolerances."); + return default_tolerances; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.position").c_str(), + active_tolerances.state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.velocity").c_str(), + active_tolerances.state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.acceleration").c_str(), + active_tolerances.state_tolerance[i].acceleration); + } + for (auto goal_tol : goal.goal_tolerance) + { + auto const joint = goal_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance does not exist. " + "Using default tolerances.") + .c_str()); + return default_tolerances; + } + auto i = std::distance(joints.cbegin(), it); + std::string interface = ""; + try + { + interface = "position"; + active_tolerances.goal_state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].position, goal_tol.position); + interface = "velocity"; + active_tolerances.goal_state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].velocity, goal_tol.velocity); + interface = "acceleration"; + active_tolerances.goal_state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].acceleration, goal_tol.acceleration); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + logger, "joint '" << joint << "' specified in goal.goal_tolerance has a invalid " + << interface << " tolerance. Using default tolerances."); + return default_tolerances; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(), + active_tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.velocity").c_str(), + active_tolerances.goal_state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.acceleration").c_str(), + active_tolerances.goal_state_tolerance[i].acceleration); + } + + return active_tolerances; +} + /** * \param state_error State error to check. * \param joint_idx Joint index for the state error diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 5aad4cb86e..112c4ad9a1 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.6.0 + 4.15.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl @@ -25,6 +25,7 @@ rsl tl_expected trajectory_msgs + urdf ament_cmake_gmock controller_manager diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9cbf6f7526..3467ec961f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -22,20 +22,17 @@ #include #include "angles/angles.h" -#include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" #include "controller_interface/helpers.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/event_handler.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server_goal_handle.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "urdf/model.h" namespace joint_trajectory_controller { @@ -58,6 +55,41 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() return CallbackReturn::ERROR; } + const std::string & urdf = get_robot_description(); + if (!urdf.empty()) + { + urdf::Model model; + if (!model.initString(urdf)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse robot description!"); + return CallbackReturn::ERROR; + } + else + { + /// initialize the URDF model and update the joint angles wraparound vector + // Configure joint position error normalization (angle_wraparound) + joints_angle_wraparound_.resize(params_.joints.size(), false); + for (size_t i = 0; i < params_.joints.size(); ++i) + { + auto urdf_joint = model.getJoint(params_.joints[i]); + if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "joint '%s' is of type continuous, use angle_wraparound.", + params_.joints[i].c_str()); + joints_angle_wraparound_[i] = true; + } + // do nothing if joint is not found in the URDF + } + RCLCPP_DEBUG(get_node()->get_logger(), "Successfully parsed URDF file"); + } + } + else + { + // empty URDF is used for some tests + RCLCPP_DEBUG(get_node()->get_logger(), "No URDF file given"); + } + return CallbackReturn::SUCCESS; } @@ -105,15 +137,16 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { return controller_interface::return_type::OK; } + auto logger = this->get_node()->get_logger(); // update dynamic parameters if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); // update the PID gains // variable use_closed_loop_pid_adapter_ is updated in on_configure only if (use_closed_loop_pid_adapter_) @@ -187,6 +220,7 @@ controller_interface::return_type JointTrajectoryController::update( bool outside_goal_tolerance = false; bool within_goal_time = true; const bool before_last_point = end_segment_itr != current_trajectory_->end(); + auto active_tol = active_tolerances_.readFromRT(); // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances @@ -194,7 +228,7 @@ controller_interface::return_type JointTrajectoryController::update( !before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) { - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); + RCLCPP_WARN(logger, "Aborted due to command timeout"); new_trajectory_msg_.reset(); new_trajectory_msg_.initRT(set_hold_position()); @@ -211,8 +245,7 @@ controller_interface::return_type JointTrajectoryController::update( if ( (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], - true /* show_errors */)) + state_error_, index, active_tol->state_tolerance[index], true /* show_errors */)) { tolerance_violated_while_moving = true; } @@ -220,14 +253,14 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], - false /* show_errors */)) + state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */)) { outside_goal_tolerance = true; - if (default_tolerances_.goal_time_tolerance != 0.0) + if (active_tol->goal_time_tolerance != 0.0) { - if (time_difference > default_tolerances_.goal_time_tolerance) + // if we exceed goal_time_tolerance set it to aborted + if (time_difference > active_tol->goal_time_tolerance) { within_goal_time = false; // print once, goal will be aborted afterwards @@ -308,7 +341,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); new_trajectory_msg_.reset(); new_trajectory_msg_.initRT(set_hold_position()); @@ -327,7 +360,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + RCLCPP_INFO(logger, "Goal reached, success!"); new_trajectory_msg_.reset(); new_trajectory_msg_.initRT(set_success_trajectory_point()); @@ -346,7 +379,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); + RCLCPP_WARN(logger, error_string.c_str()); new_trajectory_msg_.reset(); new_trajectory_msg_.initRT(set_hold_position()); @@ -356,7 +389,7 @@ controller_interface::return_type JointTrajectoryController::update( else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { // we need to ensure that there is no pending goal -> we get a race condition otherwise - RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + RCLCPP_ERROR(logger, "Holding position due to state tolerance violation"); new_trajectory_msg_.reset(); new_trajectory_msg_.initRT(set_hold_position()); @@ -364,7 +397,7 @@ controller_interface::return_type JointTrajectoryController::update( 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..."); + RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position..."); new_trajectory_msg_.reset(); new_trajectory_msg_.initRT(set_hold_position()); @@ -432,8 +465,7 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), - [](const auto & interface) + joint_interface.begin(), joint_interface.end(), [](const auto & interface) { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; @@ -503,8 +535,7 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), - [](const auto & interface) + joint_interface.begin(), joint_interface.end(), [](const auto & interface) { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; @@ -567,7 +598,7 @@ void JointTrajectoryController::query_state_service( { const auto logger = get_node()->get_logger(); // Preconditions - if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_ERROR(logger, "Can't sample trajectory. Controller is not active."); response->success = false; @@ -610,11 +641,11 @@ void JointTrajectoryController::query_state_service( controller_interface::CallbackReturn JointTrajectoryController::on_configure( const rclcpp_lifecycle::State &) { - const auto logger = get_node()->get_logger(); + auto logger = get_node()->get_logger(); if (!param_listener_) { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + RCLCPP_ERROR(logger, "Error encountered during init"); return controller_interface::CallbackReturn::ERROR; } @@ -689,14 +720,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( update_pids(); } - // Configure joint position error normalization from ROS parameters (angle_wraparound) - joints_angle_wraparound_.resize(dof_); - for (size_t i = 0; i < dof_; ++i) - { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - joints_angle_wraparound_[i] = gains.angle_wraparound; - } - if (params_.state_interfaces.empty()) { RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); @@ -762,6 +785,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(params_.state_interfaces).c_str()); // parse remaining parameters + default_tolerances_ = get_segment_tolerances(logger, params_); + active_tolerances_.initRT(default_tolerances_); const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -853,6 +878,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { + auto logger = get_node()->get_logger(); + // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); @@ -860,7 +887,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( params_ = param_listener_->get_params(); // parse remaining parameters - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); // order all joints in the storage for (const auto & interface : params_.command_interfaces) @@ -872,8 +899,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, - interface.c_str(), joint_command_interface_[index].size()); + logger, "Expected %zu '%s' command interfaces, got %zu.", dof_, interface.c_str(), + joint_command_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -886,8 +913,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, - interface.c_str(), joint_state_interface_[index].size()); + logger, "Expected %zu '%s' state interfaces, got %zu.", dof_, interface.c_str(), + joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -901,7 +928,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); - if (read_state_from_command_interfaces(state)) + if ( + params_.set_last_command_interface_value_as_state_on_activation && + read_state_from_command_interfaces(state)) { state_current_ = state; last_commanded_state_ = state; @@ -929,9 +958,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { // 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); + 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; } } @@ -1091,7 +1119,7 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback( RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); // Precondition: Running controller - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( get_node()->get_logger(), "Can't accept new action goals. Controller is not running."); @@ -1153,6 +1181,11 @@ void JointTrajectoryController::goal_accepted_callback( rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); + // Update tolerances if specified in the goal + auto logger = this->get_node()->get_logger(); + active_tolerances_.writeFromNonRT(get_segment_tolerances( + logger, default_tolerances_, *(goal_handle->get_goal()), params_.joints)); + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); @@ -1348,6 +1381,24 @@ bool JointTrajectoryController::validate_trajectory_msg( return false; } + const auto trajectory_start_time = static_cast(trajectory.header.stamp); + // If the starting time it set to 0.0, it means the controller should start it now. + // Otherwise we check if the trajectory ends before the current time, + // in which case it can be ignored. + if (trajectory_start_time.seconds() != 0.0) + { + auto const trajectory_end_time = + trajectory_start_time + trajectory.points.back().time_from_start; + if (trajectory_end_time < get_node()->now()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received trajectory with non-zero start time (%f) that ends in the past (%f)", + trajectory_start_time.seconds(), trajectory_end_time.seconds()); + 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) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index ded5bb7ca3..14b71f0711 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -64,6 +64,11 @@ joint_trajectory_controller: default_value: false, description: "Allow integration in goal trajectories to accept goals without position or velocity specified", } + set_last_command_interface_value_as_state_on_activation: { + type: bool, + default_value: true, + description: "When set to true, the last command interface value is used as both the current state and the last commanded state upon activation. When set to false, the current state is used for both.", + } action_monitor_rate: { type: double, default_value: 20.0, @@ -122,15 +127,6 @@ joint_trajectory_controller: default_value: 0.0, description: "Feed-forward scaling :math:`k_{ff}` of velocity" } - angle_wraparound: { - type: bool, - default_value: false, - description: 'For joints that wrap around (without end stop, ie. are continuous), - where the shortest rotation to the target position is the desired motion. - If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. - Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured - position :math:`s` from the state interface.' - } constraints: stopped_velocity_tolerance: { type: double, diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 0ed7f2ff13..54f785a070 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -20,7 +20,6 @@ #include "hardware_interface/macros.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" -#include "std_msgs/msg/header.hpp" namespace joint_trajectory_controller { diff --git a/joint_trajectory_controller/test/test_assets.hpp b/joint_trajectory_controller/test/test_assets.hpp new file mode 100644 index 0000000000..df6f2a37c7 --- /dev/null +++ b/joint_trajectory_controller/test/test_assets.hpp @@ -0,0 +1,276 @@ +// Copyright 2023 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_ASSETS_HPP_ +#define TEST_ASSETS_HPP_ + +namespace test_trajectory_controllers +{ +const auto urdf_rrrbot_revolute = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_rrrbot_continuous = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +} // namespace test_trajectory_controllers + +#endif // TEST_ASSETS_HPP_ diff --git a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp index eb1a3691e6..8d0fbb3e75 100644 --- a/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include -#include "gmock/gmock.h" +#include #include "controller_manager/controller_manager.hpp" #include "hardware_interface/resource_manager.hpp" @@ -31,9 +31,7 @@ TEST(TestLoadJointStateController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp new file mode 100644 index 0000000000..106a9a761f --- /dev/null +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -0,0 +1,440 @@ +// Copyright 2024 Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +#include "joint_trajectory_controller/tolerances.hpp" +#include "test_trajectory_controller_utils.hpp" + +using joint_trajectory_controller::SegmentTolerances; +using trajectory_msgs::msg::JointTrajectoryPoint; + +std::vector joint_names_ = {"joint1", "joint2", "joint3"}; + +control_msgs::action::FollowJointTrajectory_Goal prepareGoalMsg( + const std::vector & points, double goal_time_tolerance, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) +{ + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; + goal_msg.trajectory.joint_names = joint_names_; + goal_msg.trajectory.points = points; + + return goal_msg; +} +class TestTolerancesFixture : public ::testing::Test +{ +protected: + SegmentTolerances default_tolerances; + joint_trajectory_controller::Params params; + std::vector joint_names_; + rclcpp::Logger logger = rclcpp::get_logger("TestTolerancesFixture"); + + void SetUp() override + { + // Initialize joint_names_ with some test data + joint_names_ = {"joint1", "joint2", "joint3"}; + + // Initialize default_tolerances and params with common setup for all tests + // TODO(anyone) fill params and use + // SegmentTolerances get_segment_tolerances(Params const & params) instead + default_tolerances.goal_time_tolerance = default_goal_time; + default_tolerances.state_tolerance.resize(joint_names_.size()); + default_tolerances.goal_state_tolerance.resize(joint_names_.size()); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + default_tolerances.state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).velocity = stopped_velocity_tolerance; + } + params.joints = joint_names_; + } + + void TearDown() override + { + // Cleanup code if necessary + } +}; + +TEST_F(TestTolerancesFixture, test_get_segment_tolerances) +{ + // send goal with nonzero tolerances, are they accepted? + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 2.0, path_tolerance, goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); +} + +// send goal with deactivated tolerances (-1) +TEST_F(TestTolerancesFixture, test_deactivate_tolerances) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = -1.0; + tolerance.velocity = -1.0; + tolerance.acceleration = -1.0; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, -1.0, path_tolerance, goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 0.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + +// send goal with invalid tolerances, are the default ones used? +TEST_F(TestTolerancesFixture, test_invalid_tolerances) +{ + { + SCOPED_TRACE("negative goal_time_tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, -123.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } +} +TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); +} +TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); +} diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 6e0c53ac77..b0d7ad8e18 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -12,15 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include + #include #include #include -#include "gmock/gmock.h" - -#include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 87d557df1b..164f7d0e11 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -15,40 +15,32 @@ #ifndef _MSC_VER #include #endif -#include #include #include #include #include -#include #include #include -#include #include #include -#include "action_msgs/msg/goal_status_array.hpp" #include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp" -#include "controller_interface/controller_interface.hpp" #include "gtest/gtest.h" -#include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_action/client.hpp" #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/create_client.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "test_trajectory_controller_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "test_trajectory_controller_utils.hpp" + using std::placeholders::_1; using std::placeholders::_2; using test_trajectory_controllers::TestableJointTrajectoryController; @@ -153,10 +145,17 @@ class TestTrajectoryActions : public TrajectoryControllerTest using GoalOptions = rclcpp_action::Client::SendGoalOptions; std::shared_future sendActionGoal( - const std::vector & points, double timeout, const GoalOptions & opt) + const std::vector & points, double goal_time_tolerance, + const GoalOptions & opt, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; - goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; goal_msg.trajectory.points = points; @@ -489,6 +488,163 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) expectCommandPoint(points_positions.at(1)); } +/** + * No need for parameterized tests + */ +TEST_F(TestTrajectoryActions, test_tolerances_via_actions) +{ + // set tolerance parameters + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", 0.1), + rclcpp::Parameter("constraints.joint2.goal", 0.1), + rclcpp::Parameter("constraints.joint3.goal", 0.1), + rclcpp::Parameter("constraints.goal_time", default_goal_time), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.1), + rclcpp::Parameter("constraints.joint1.trajectory", 0.1), + rclcpp::Parameter("constraints.joint2.trajectory", 0.1), + rclcpp::Parameter("constraints.joint3.trajectory", 0.1)}; + + SetUpExecutor(params); + + { + SCOPED_TRACE("Check default values"); + SetUpControllerHardware(); + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 0.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + expectDefaultTolerances(active_tolerances); + } + + // send goal with nonzero tolerances, are they accepted? + { + SetUpControllerHardware(); + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 2.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); + } + + // send goal without tolerances again, are the default ones used? + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 0.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + expectDefaultTolerances(active_tolerances); + } +} + TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) { // set joint tolerance parameters @@ -683,7 +839,8 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) { std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; SetUpExecutor(params); SetUpControllerHardware(); @@ -733,7 +890,8 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_tr TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false) { std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false)}; + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; SetUpExecutor(params); SetUpControllerHardware(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 2bfe6f150d..f188c0f04b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -12,46 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include -#include #include -#include -#include #include -#include #include #include #include "builtin_interfaces/msg/duration.hpp" -#include "builtin_interfaces/msg/time.hpp" -#include "controller_interface/controller_interface.hpp" -#include "hardware_interface/resource_manager.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" -#include "rclcpp/event_handler.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" -#include "rclcpp/publisher.hpp" -#include "rclcpp/qos.hpp" -#include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "test_assets.hpp" #include "test_trajectory_controller_utils.hpp" using lifecycle_msgs::msg::State; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; -void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } +TEST_P(TrajectoryControllerTestParameterized, invalid_robot_description) +{ + ASSERT_EQ( + controller_interface::return_type::ERROR, + SetUpTrajectoryControllerLocal({}, "")); +} TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { @@ -60,8 +53,6 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); - // const auto future_handle_ = std::async(std::launch::async, spin, &executor); - const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); @@ -257,7 +248,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); + subscribeToState(executor); updateController(); // Spin to receive latest state @@ -463,14 +454,15 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) const double EPS = 1e-6; /** - * @brief check if calculated trajectory error is correct with angle wraparound=true + * @brief check if calculated trajectory error is correct (angle wraparound) for continuous joints */ 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 */); + executor, params, true, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_continuous); size_t n_joints = joint_names_.size(); @@ -554,14 +546,15 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru } /** - * @brief check if calculated trajectory error is correct with angle wraparound=false + * @brief check if calculated trajectory error is correct (no angle wraparound) for revolute joints */ 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 */); + executor, params, true, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); size_t n_joints = joint_names_.size(); @@ -636,16 +629,17 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal } /** - * @brief check if position error of revolute joints are wrapped around if not configured so + * @brief check if position error of revolute joints aren't wrapped around (state topic) */ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; std::vector params = {}; - bool angle_wraparound = false; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound); - subscribeToState(); + SetUpAndActivateTrajectoryController( + executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); + subscribeToState(executor); size_t n_joints = joint_names_.size(); @@ -737,14 +731,16 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun } /** - * @brief check if position error of revolute joints are wrapped around if configured so + * @brief check if position error of continuous joints are wrapped around (state topic) */ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; std::vector params = {}; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); + SetUpAndActivateTrajectoryController( + executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_continuous); size_t n_joints = joint_names_.size(); @@ -1383,6 +1379,17 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) traj_msg = good_traj_msg; traj_msg.points.push_back(traj_msg.points.front()); EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // End time in the past + traj_msg = good_traj_msg; + traj_msg.header.stamp = rclcpp::Time(1); + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + + // End time in the future + traj_msg = good_traj_msg; + traj_msg.header.stamp = traj_controller_->get_node()->now(); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(10); + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); } /** @@ -1844,7 +1851,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co std::vector initial_acc_cmd{3, std::numeric_limits::quiet_NaN()}; SetUpAndActivateTrajectoryController( - executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, initial_acc_cmd); // no call of update method, so the values should be read from state interfaces @@ -1888,7 +1895,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0); } SetUpAndActivateTrajectoryController( - executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, initial_acc_cmd); // no call of update method, so the values should be read from command interfaces diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 485cdff884..be14bf8dcb 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -25,6 +25,8 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "joint_trajectory_controller/tolerances.hpp" +#include "ros2_control_test_assets/descriptions.hpp" namespace { @@ -38,11 +40,46 @@ const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; +const double default_goal_time = 0.1; +const double stopped_velocity_tolerance = 0.1; + +[[maybe_unused]] void expectDefaultTolerances( + joint_trajectory_controller::SegmentTolerances active_tolerances) +{ + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(0).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(1).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(2).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + bool is_same_sign_or_zero(double val1, double val2) { return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); } - } // namespace namespace test_trajectory_controllers @@ -58,11 +95,6 @@ class TestableJointTrajectoryController const rclcpp_lifecycle::State & previous_state) override { auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure(previous_state); - // this class can still be useful without the wait set - if (joint_command_subscriber_) - { - joint_cmd_sub_wait_set_.add_subscription(joint_command_subscriber_); - } return ret; } @@ -72,19 +104,17 @@ class TestableJointTrajectoryController * @brief wait_for_trajectory block until a new JointTrajectory is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function - * - * @return true if new JointTrajectory msg was received, false if timeout */ - bool wait_for_trajectory( + void wait_for_trajectory( rclcpp::Executor & executor, - const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{10}) { - bool success = joint_cmd_sub_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } void set_joint_names(const std::vector & joint_names) @@ -138,6 +168,11 @@ class TestableJointTrajectoryController bool is_open_loop() const { return params_.open_loop_control; } + joint_trajectory_controller::SegmentTolerances get_active_tolerances() + { + return *(active_tolerances_.readFromRT()); + } + std::vector get_pids() const { return pids_; } joint_trajectory_controller::SegmentTolerances get_tolerances() const @@ -182,7 +217,6 @@ class TestableJointTrajectoryController } } - rclcpp::WaitSet joint_cmd_sub_wait_set_; rclcpp::NodeOptions node_options_; }; @@ -215,9 +249,10 @@ class TrajectoryControllerTest : public ::testing::Test } void SetUpTrajectoryController( - rclcpp::Executor & executor, const std::vector & parameters = {}) + rclcpp::Executor & executor, const std::vector & parameters = {}, + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { - auto ret = SetUpTrajectoryControllerLocal(parameters); + auto ret = SetUpTrajectoryControllerLocal(parameters, urdf); if (ret != controller_interface::return_type::OK) { FAIL(); @@ -226,7 +261,8 @@ class TrajectoryControllerTest : public ::testing::Test } controller_interface::return_type SetUpTrajectoryControllerLocal( - const std::vector & parameters = {}) + const std::vector & parameters = {}, + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { traj_controller_ = std::make_shared(); @@ -241,11 +277,10 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->set_node_options(node_options); return traj_controller_->init( - controller_name_, "", 0, "", traj_controller_->define_custom_node_options()); + controller_name_, urdf, 0, "", traj_controller_->define_custom_node_options()); } - void SetPidParameters( - double p_value = 0.0, double ff_value = 1.0, bool angle_wraparound_value = false) + void SetPidParameters(double p_value = 0.0, double ff_value = 1.0) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -258,27 +293,24 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_value); - const rclcpp::Parameter angle_wraparound( - prefix + ".angle_wraparound", angle_wraparound_value); - node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, angle_wraparound}); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); } } void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, - bool angle_wraparound = false, const std::vector initial_pos_joints = INITIAL_POS_JOINTS, const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, - const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) + const std::vector initial_eff_joints = INITIAL_EFF_JOINTS, + const std::string & urdf = ros2_control_test_assets::minimal_robot_urdf) { auto has_nonzero_vel_param = std::find_if( - parameters.begin(), parameters.end(), - [](const rclcpp::Parameter & param) { - return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; - }) != parameters.end(); + parameters.begin(), parameters.end(), [](const rclcpp::Parameter & param) + { return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; }) != + parameters.end(); std::vector parameters_local = parameters; if (!has_nonzero_vel_param) @@ -287,10 +319,10 @@ class TrajectoryControllerTest : public ::testing::Test parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true); } // read-only parameters have to be set before init -> won't be read otherwise - SetUpTrajectoryController(executor, parameters_local); + SetUpTrajectoryController(executor, parameters_local, urdf); // set pid parameters before configure - SetPidParameters(k_p, ff, angle_wraparound); + SetPidParameters(k_p, ff); traj_controller_->get_node()->configure(); ActivateTrajectoryController( @@ -361,7 +393,7 @@ class TrajectoryControllerTest : public ::testing::Test static void TearDownTestCase() { rclcpp::shutdown(); } - void subscribeToState() + void subscribeToState(rclcpp::Executor & executor) { auto traj_lifecycle_node = traj_controller_->get_node(); @@ -378,6 +410,14 @@ class TrajectoryControllerTest : public ::testing::Test std::lock_guard guard(state_mutex_); state_msg_ = msg; }); + + const auto timeout = std::chrono::milliseconds{10}; + const auto until = traj_lifecycle_node->get_clock()->now() + timeout; + while (traj_lifecycle_node->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } } /// Publish trajectory msgs with multiple points diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst new file mode 100644 index 0000000000..a3024fbf62 --- /dev/null +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -0,0 +1,210 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package parallel_gripper_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* [GripperController] Fix failing tests (`#1210 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Add parallel_gripper_controller, configure gripper speed and effort with hardware interface (`#1002 `_) +* Contributors: Paul Gesel, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt new file mode 100644 index 0000000000..cb0f4fafc9 --- /dev/null +++ b/parallel_gripper_controller/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.16) +project(parallel_gripper_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(parallel_gripper_action_controller_parameters + src/gripper_action_controller_parameters.yaml +) + +add_library(parallel_gripper_action_controller SHARED + src/parallel_gripper_action_controller.cpp +) +target_compile_features(parallel_gripper_action_controller PUBLIC cxx_std_17) +target_include_directories(parallel_gripper_action_controller PUBLIC + $ + $ +) +target_link_libraries(parallel_gripper_action_controller PUBLIC + parallel_gripper_action_controller_parameters +) +ament_target_dependencies(parallel_gripper_action_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +pluginlib_export_plugin_description_file(controller_interface ros_control_plugins.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_parallel_gripper_action_controllers test/test_load_parallel_gripper_action_controller.cpp) + target_include_directories(test_load_parallel_gripper_action_controllers PRIVATE include) + ament_target_dependencies(test_load_parallel_gripper_action_controllers + controller_manager + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_parallel_gripper_controller + test/test_parallel_gripper_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/gripper_action_controller_params.yaml) + target_include_directories(test_parallel_gripper_controller PRIVATE include) + target_link_libraries(test_parallel_gripper_controller + parallel_gripper_action_controller + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/parallel_gripper_action_controller +) +install( + TARGETS + parallel_gripper_action_controller + parallel_gripper_action_controller_parameters + EXPORT export_parallel_gripper_action_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_parallel_gripper_action_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/parallel_gripper_controller/doc/userdoc.rst b/parallel_gripper_controller/doc/userdoc.rst new file mode 100644 index 0000000000..5c04eb5f90 --- /dev/null +++ b/parallel_gripper_controller/doc/userdoc.rst @@ -0,0 +1,19 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/parallel_gripper_controller/doc/userdoc.rst + +.. _parallel_gripper_controller_userdoc: + +Parallel Gripper Action Controller +----------------------------------- + +Controller for executing a ``ParallelGripperCommand`` action for simple parallel grippers. +This controller supports grippers that offer position only control as well as grippers that allow configuring the velocity and effort. +By default, the controller will only claim the ``{joint}/position`` interface for control. +The velocity and effort interfaces can be optionally claimed by setting the ``max_velocity_interface`` and ``max_effort_interface`` parameter, respectively. +By default, the controller will try to claim position and velocity state interfaces. +The claimed state interfaces can be configured by setting the ``state_interfaces`` parameter. + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. + +.. generate_parameter_library_details:: ../src/gripper_action_controller_parameters.yaml diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp new file mode 100644 index 0000000000..d303990b89 --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller.hpp @@ -0,0 +1,174 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian + +#ifndef PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ + +// C++ standard +#include +#include +#include +#include + +// ROS +#include "rclcpp/rclcpp.hpp" + +// ROS messages +#include "control_msgs/action/parallel_gripper_command.hpp" + +// rclcpp_action +#include "rclcpp_action/create_server.hpp" + +// ros_controls +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "parallel_gripper_controller/visibility_control.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_server_goal_handle.h" + +// Project +#include "parallel_gripper_action_controller_parameters.hpp" + +namespace parallel_gripper_action_controller +{ +/** + * \brief Controller for executing a `ParallelGripperCommand` action. + * + * \tparam HardwareInterface Controller hardware interface. Currently \p + * hardware_interface::HW_IF_POSITION and \p + * hardware_interface::HW_IF_EFFORT are supported out-of-the-box. + */ + +class GripperActionController : public controller_interface::ControllerInterface +{ +public: + /** + * \brief Store position and max effort in struct to allow easier realtime + * buffer usage + */ + struct Commands + { + double position_cmd_; // Commanded position + double max_velocity_; // Desired max gripper velocity + double max_effort_; // Desired max allowed effort + }; + + GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController(); + + /** + * @brief command_interface_configuration This controller requires the + * position command interfaces for the controlled joints + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @brief command_interface_configuration This controller requires the + * position and velocity state interfaces for the controlled joints + */ + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_ACTION_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + realtime_tools::RealtimeBuffer command_; + // pre-allocated memory that is re-used to set the realtime buffer + Commands command_struct_, command_struct_rt_; + +protected: + using GripperCommandAction = control_msgs::action::ParallelGripperCommand; + using ActionServer = rclcpp_action::Server; + using ActionServerPtr = ActionServer::SharedPtr; + using GoalHandle = rclcpp_action::ServerGoalHandle; + using RealtimeGoalHandle = + realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + bool update_hold_position_; + + bool verbose_ = false; ///< Hard coded verbose flag to help in debugging + std::string name_; ///< Controller name. + std::optional> + joint_command_interface_; + std::optional> + effort_interface_; + std::optional> + speed_interface_; + std::optional> + joint_position_state_interface_; + std::optional> + joint_velocity_state_interface_; + + std::shared_ptr param_listener_; + Params params_; + + RealtimeGoalHandleBuffer + rt_active_goal_; ///< Container for the currently active action goal, if any. + control_msgs::action::ParallelGripperCommand::Result::SharedPtr pre_alloc_result_; + + rclcpp::Duration action_monitor_period_; + + // ROS API + ActionServerPtr action_server_; + + rclcpp::TimerBase::SharedPtr goal_handle_timer_; + + rclcpp_action::GoalResponse goal_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle); + + void accepted_callback(std::shared_ptr goal_handle); + + void preempt_active_goal(); + + void set_hold_position(); + + rclcpp::Time last_movement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); ///< Store stall time + double computed_command_; ///< Computed command + + /** + * \brief Check for success and publish appropriate result and feedback. + **/ + void check_for_success( + const rclcpp::Time & time, double error_position, double current_position, + double current_velocity); +}; + +} // namespace parallel_gripper_action_controller + +#include "parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp" + +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_HPP_ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp new file mode 100644 index 0000000000..d37cb87fc8 --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -0,0 +1,395 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian, Stu Glaser + +#ifndef PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ + +#include +#include +#include + +#include +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" + +namespace parallel_gripper_action_controller +{ + +void GripperActionController::preempt_active_goal() +{ + // Cancels the currently active goal + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) + { + // Marks the current goal as canceled + active_goal->setCanceled(std::make_shared()); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } +} + +controller_interface::CallbackReturn GripperActionController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type GripperActionController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + command_struct_rt_ = *(command_.readFromRT()); + + const double current_position = joint_position_state_interface_->get().get_value(); + const double current_velocity = joint_velocity_state_interface_->get().get_value(); + const double error_position = command_struct_rt_.position_cmd_ - current_position; + + check_for_success(get_node()->now(), error_position, current_position, current_velocity); + + joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_); + if (speed_interface_.has_value()) + { + speed_interface_->get().set_value(command_struct_rt_.max_velocity_); + } + if (effort_interface_.has_value()) + { + effort_interface_->get().set_value(command_struct_rt_.max_effort_); + } + + return controller_interface::return_type::OK; +} + +rclcpp_action::GoalResponse GripperActionController::goal_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr goal_handle) +{ + if (goal_handle->command.position.size() != 1) + { + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + RCLCPP_ERROR( + get_node()->get_logger(), + "Received action goal with wrong number of position values, expects 1, got %zu", + goal_handle->command.position.size()); + return rclcpp_action::GoalResponse::REJECT; + } + + RCLCPP_INFO(get_node()->get_logger(), "Received & accepted new action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void GripperActionController::accepted_callback( + std::shared_ptr goal_handle) // Try to update goal +{ + auto rt_goal = std::make_shared(goal_handle); + + // Accept new goal + preempt_active_goal(); + + // This is the non-realtime command_struct + // We use command_ for sharing + command_struct_.position_cmd_ = goal_handle->get_goal()->command.position[0]; + if (!params_.max_velocity_interface.empty() && !goal_handle->get_goal()->command.velocity.empty()) + { + command_struct_.max_velocity_ = goal_handle->get_goal()->command.velocity[0]; + } + else + { + command_struct_.max_velocity_ = params_.max_velocity; + } + if (!params_.max_effort_interface.empty() && !goal_handle->get_goal()->command.effort.empty()) + { + command_struct_.max_effort_ = goal_handle->get_goal()->command.effort[0]; + } + else + { + command_struct_.max_effort_ = params_.max_effort; + } + command_.writeFromNonRT(command_struct_); + + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; + + last_movement_time_ = get_node()->now(); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list + goal_handle_timer_.reset(); + + // Setup goal status checking timer + goal_handle_timer_ = get_node()->create_wall_timer( + action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); +} + +rclcpp_action::CancelResponse GripperActionController::cancel_callback( + const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); + + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) + { + // Enter hold current position mode + set_hold_position(); + + RCLCPP_INFO( + get_node()->get_logger(), "Canceling active action goal because cancel callback received."); + + // Mark the current goal as canceled + auto action_res = std::make_shared(); + active_goal->setCanceled(action_res); + // Reset current goal + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GripperActionController::set_hold_position() +{ + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_effort_ = params_.max_effort; + command_struct_.max_velocity_ = params_.max_velocity; + command_.writeFromNonRT(command_struct_); +} + +void GripperActionController::check_for_success( + const rclcpp::Time & time, double error_position, double current_position, + double current_velocity) +{ + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (!active_goal) + { + return; + } + + if (fabs(error_position) < params_.goal_tolerance) + { + pre_alloc_result_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = current_position; + pre_alloc_result_->reached_goal = true; + pre_alloc_result_->stalled = false; + RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); + active_goal->setSucceeded(pre_alloc_result_); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + else + { + if (fabs(current_velocity) > params_.stall_velocity_threshold) + { + last_movement_time_ = time; + } + else if ((time - last_movement_time_).seconds() > params_.stall_timeout) + { + pre_alloc_result_->state.effort[0] = computed_command_; + pre_alloc_result_->state.position[0] = current_position; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = true; + + if (params_.allow_stalling) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); + active_goal->setSucceeded(pre_alloc_result_); + } + else + { + RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); + active_goal->setAborted(pre_alloc_result_); + } + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + } + } +} + +controller_interface::CallbackReturn GripperActionController::on_configure( + const rclcpp_lifecycle::State &) +{ + const auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); + + // Action status checking update rate + action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); + RCLCPP_INFO_STREAM( + logger, "Action status changes will be monitored at " << params_.action_monitor_rate << "Hz."); + + // Controlled joint + if (params_.joint.empty()) + { + RCLCPP_ERROR(logger, "Joint name cannot be empty"); + return controller_interface::CallbackReturn::ERROR; + } + RCLCPP_INFO(logger, "Joint name is : %s", params_.joint.c_str()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GripperActionController::on_activate( + const rclcpp_lifecycle::State &) +{ + auto command_interface_it = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [](const hardware_interface::LoanedCommandInterface & command_interface) + { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); + if (command_interface_it == command_interfaces_.end()) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Expected 1 " << hardware_interface::HW_IF_POSITION << " command interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (command_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Command interface is different than joint name `" + << command_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + const auto position_state_interface_it = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); + if (position_state_interface_it == state_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (position_state_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Position state interface is different than joint name `" + << position_state_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + const auto velocity_state_interface_it = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [](const hardware_interface::LoanedStateInterface & state_interface) + { return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; }); + if (velocity_state_interface_it == state_interfaces_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface"); + return controller_interface::CallbackReturn::ERROR; + } + if (velocity_state_interface_it->get_prefix_name() != params_.joint) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Velocity command interface is different than joint name `" + << velocity_state_interface_it->get_prefix_name() << "` != `" + << params_.joint << "`"); + return controller_interface::CallbackReturn::ERROR; + } + + joint_command_interface_ = *command_interface_it; + joint_position_state_interface_ = *position_state_interface_it; + joint_velocity_state_interface_ = *velocity_state_interface_it; + + for (auto & interface : command_interfaces_) + { + if (interface.get_interface_name() == "set_gripper_max_effort") + { + effort_interface_ = interface; + } + else if (interface.get_interface_name() == "set_gripper_max_velocity") + { + speed_interface_ = interface; + } + } + + // Command - non RT version + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.max_effort_ = params_.max_effort; + command_struct_.max_velocity_ = params_.max_velocity; + command_.initRT(command_struct_); + + // Result + pre_alloc_result_ = std::make_shared(); + pre_alloc_result_->state.position.resize(1); + pre_alloc_result_->state.effort.resize(1); + pre_alloc_result_->state.position[0] = command_struct_.position_cmd_; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; + + // Action interface + action_server_ = rclcpp_action::create_server( + get_node(), "~/gripper_cmd", + std::bind( + &GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&GripperActionController::cancel_callback, this, std::placeholders::_1), + std::bind(&GripperActionController::accepted_callback, this, std::placeholders::_1)); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GripperActionController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + joint_command_interface_ = std::nullopt; + joint_position_state_interface_ = std::nullopt; + joint_velocity_state_interface_ = std::nullopt; + release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +GripperActionController::command_interface_configuration() const +{ + std::vector names = {params_.joint + "/" + hardware_interface::HW_IF_POSITION}; + if (!params_.max_effort_interface.empty()) + { + names.push_back({params_.max_effort_interface}); + } + if (!params_.max_velocity_interface.empty()) + { + names.push_back({params_.max_velocity_interface}); + } + + return {controller_interface::interface_configuration_type::INDIVIDUAL, names}; +} + +controller_interface::InterfaceConfiguration +GripperActionController::state_interface_configuration() const +{ + std::vector interface_names; + for (const auto & interface : params_.state_interfaces) + { + interface_names.push_back(params_.joint + "/" + interface); + } + return {controller_interface::interface_configuration_type::INDIVIDUAL, interface_names}; +} + +GripperActionController::GripperActionController() +: controller_interface::ControllerInterface(), + action_monitor_period_(rclcpp::Duration::from_seconds(0)) +{ +} + +} // namespace parallel_gripper_action_controller + +#endif // PARALLEL_GRIPPER_CONTROLLER__PARALLEL_GRIPPER_ACTION_CONTROLLER_IMPL_HPP_ diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp new file mode 100644 index 0000000000..63e5c8e1c7 --- /dev/null +++ b/parallel_gripper_controller/include/parallel_gripper_controller/visibility_control.hpp @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ +#define PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec(dllexport) +#define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#else +#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GRIPPER_ACTION_CONTROLLER_PUBLIC +#define GRIPPER_ACTION_CONTROLLER_LOCAL +#endif +#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // PARALLEL_GRIPPER_CONTROLLER__VISIBILITY_CONTROL_HPP_ diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml new file mode 100644 index 0000000000..b396ba7b85 --- /dev/null +++ b/parallel_gripper_controller/package.xml @@ -0,0 +1,35 @@ + + + + parallel_gripper_controller + 4.15.0 + The parallel_gripper_controller package + Bence Magyar + + Apache License 2.0 + + Sachin Chitta + + ament_cmake + + backward_ros + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_action + realtime_tools + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/parallel_gripper_controller/ros_control_plugins.xml b/parallel_gripper_controller/ros_control_plugins.xml new file mode 100644 index 0000000000..dcd9609ba7 --- /dev/null +++ b/parallel_gripper_controller/ros_control_plugins.xml @@ -0,0 +1,10 @@ + + + + + + + + diff --git a/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml new file mode 100644 index 0000000000..10b0df0958 --- /dev/null +++ b/parallel_gripper_controller/src/gripper_action_controller_parameters.yaml @@ -0,0 +1,72 @@ +parallel_gripper_action_controller: + action_monitor_rate: { + type: double, + default_value: 20.0, + description: "Hz", + read_only: true, + validation: { + gt_eq: [ 0.1 ] + }, + } + joint: { + type: string, + read_only: true, + validation: { + not_empty<>: null + } + } + state_interfaces: { + type: string_array, + default_value: [position, velocity], + } + goal_tolerance: { + type: double, + default_value: 0.01, + validation: { + gt_eq: [ 0.0 ] + }, + } + allow_stalling: { + type: bool, + description: "Allow stalling will make the action server return success if the gripper stalls when moving to the goal", + default_value: false, + } + stall_velocity_threshold: { + type: double, + description: "stall velocity threshold", + default_value: 0.001, + } + stall_timeout: { + type: double, + description: "stall timeout", + default_value: 1.0, + validation: { + gt_eq: [ 0.0 ] + }, + } + max_effort_interface: { + type: string, + description: "Controller will claim the specified effort interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", + } + max_effort: { + type: double, + default_value: 10.0, + description: "Default effort used for grasping (Newtons)", + validation: { + gt_eq: [ 0.0 ] + }, + } + max_velocity_interface: { + type: string, + description: "Controller will claim the speed specified interface, e.g. robotiq_85_left_knuckle_joint/max_effort_interface.", + default_value: "", + } + max_velocity: { + type: double, + default_value: 0.1, + description: "Default target velocity used for grasping (meters/second)", + validation: { + gt_eq: [ 0.0 ] + }, + } diff --git a/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp new file mode 100644 index 0000000000..3a6d028ddf --- /dev/null +++ b/parallel_gripper_controller/src/parallel_gripper_action_controller.cpp @@ -0,0 +1,25 @@ +// Copyright 2014, SRI International +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sachin Chitta + +// Project +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + parallel_gripper_action_controller::GripperActionController, + controller_interface::ControllerInterface) diff --git a/parallel_gripper_controller/test/gripper_action_controller_params.yaml b/parallel_gripper_controller/test/gripper_action_controller_params.yaml new file mode 100644 index 0000000000..521960e18c --- /dev/null +++ b/parallel_gripper_controller/test/gripper_action_controller_params.yaml @@ -0,0 +1,7 @@ +test_gripper_action_position_controller: + ros__parameters: + joint: "joint1" + +test_gripper_action_position_controller_empty_joint: + ros__parameters: + joint: "" diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp new file mode 100644 index 0000000000..6453539fa0 --- /dev/null +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -0,0 +1,48 @@ +// Copyright 2020 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGripperActionControllers, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/gripper_action_controller_params.yaml"; + + cm.set_parameter({"test_gripper_action_position_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_gripper_action_position_controller.type", + "parallel_gripper_action_controller/GripperActionController"}); + + ASSERT_NE(cm.load_controller("test_gripper_action_position_controller"), nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp new file mode 100644 index 0000000000..417d87d40f --- /dev/null +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -0,0 +1,169 @@ +// Copyright 2020 PAL Robotics SL. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "test_parallel_gripper_controller.hpp" + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using GripperCommandAction = control_msgs::action::ParallelGripperCommand; +using GoalHandle = rclcpp_action::ServerGoalHandle; +using testing::SizeIs; +using testing::UnorderedElementsAre; + +void GripperControllerTest::SetUpTestCase() {} + +void GripperControllerTest::TearDownTestCase() {} + +void GripperControllerTest::SetUp() +{ + // initialize controller + controller_ = std::make_unique(); +} + +void GripperControllerTest::TearDown() { controller_.reset(nullptr); } + +void GripperControllerTest::SetUpController( + const std::string & controller_name = "test_gripper_action_position_controller", + controller_interface::return_type expected_result = controller_interface::return_type::OK) +{ + const auto result = + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()); + ASSERT_EQ(result, expected_result); + + std::vector command_ifs; + command_ifs.emplace_back(this->joint_1_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); +} + +TEST_F(GripperControllerTest, ParametersNotSet) +{ + this->SetUpController( + "test_gripper_action_position_controller_no_parameters", + controller_interface::return_type::ERROR); + + // configure failed, 'joints' parameter not set + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, JointParameterIsEmpty) +{ + this->SetUpController( + "test_gripper_action_position_controller_empty_joint", + controller_interface::return_type::ERROR); + + // configure failed, 'joints' is empty + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(GripperControllerTest, ConfigureParamsSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + rclcpp::spin_some(this->controller_->get_node()->get_node_base_interface()); + + // configure successful + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // check interface configuration + auto cmd_if_conf = this->controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(1lu)); + ASSERT_THAT( + cmd_if_conf.names, + UnorderedElementsAre(std::string("joint1/") + hardware_interface::HW_IF_POSITION)); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto state_if_conf = this->controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(2lu)); + ASSERT_THAT(state_if_conf.names, UnorderedElementsAre("joint1/position", "joint1/velocity")); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); +} + +TEST_F(GripperControllerTest, ActivateSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + // activate successful + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) +{ + this->SetUpController(); + + this->controller_->get_node()->set_parameter({"joint", "joint1"}); + + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_deactivate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // re-assign interfaces + std::vector command_ifs; + command_ifs.emplace_back(this->joint_1_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + this->controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + ASSERT_EQ( + this->controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + this->controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp new file mode 100644 index 0000000000..d206097782 --- /dev/null +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -0,0 +1,67 @@ +// Copyright 2022 ros2_control development team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ +#define TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "parallel_gripper_controller/parallel_gripper_action_controller.hpp" + +namespace +{ +// subclassing and friending so we can access member variables +class FriendGripperController : public parallel_gripper_action_controller::GripperActionController +{ + FRIEND_TEST(GripperControllerTest, CommandSuccessTest); +}; + +class GripperControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController( + const std::string & controller_name, controller_interface::return_type expected_result); + void SetUpHandles(); + +protected: + std::unique_ptr controller_; + + // dummy joint state values used for tests + const std::string joint_name_ = "joint1"; + std::vector joint_states_ = {1.1, 2.1}; + std::vector joint_commands_ = {3.1}; + + hardware_interface::StateInterface joint_1_pos_state_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]}; + hardware_interface::StateInterface joint_1_vel_state_{ + joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]}; + hardware_interface::CommandInterface joint_1_cmd_{ + joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]}; +}; + +} // anonymous namespace + +#endif // TEST_PARALLEL_GRIPPER_CONTROLLER_HPP_ diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 7d5d248576..5a27471199 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,54 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + +4.14.0 (2024-09-11) +------------------- +* [PID Controller] Export state interfaces for easier chaining with other controllers (`#1214 `_) +* Contributors: Sai Kishor Kothakota + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ +* [PID] Add example yaml to docs (`#951 `_) +* Contributors: Christoph Fröhlich + +4.7.0 (2024-03-22) +------------------ +* Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) +* Added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Chris Lalancette, Sai Kishor Kothakota, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 6c9e00ef8b..f9a9abce89 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -5,6 +5,16 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() +if(WIN32) + add_compile_definitions( + # For math constants + _USE_MATH_DEFINES + # Minimize Windows namespace collision + NOMINMAX + WIN32_LEAN_AND_MEAN + ) +endif() + set(THIS_PACKAGE_INCLUDE_DEPENDS angles control_msgs diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index 5570f523fe..fc94357ab3 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -83,4 +83,21 @@ Parameters The PID controller uses the `generate_parameter_library `_ to handle its parameters. +List of parameters +========================= .. generate_parameter_library_details:: ../src/pid_controller.yaml + + +An example parameter file +========================= + + +An example parameter file for this controller can be found in `the test folder (standalone) `_: + +.. literalinclude:: ../test/pid_controller_params.yaml + :language: yaml + +or as `preceding controller `_: + +.. literalinclude:: ../test/pid_controller_preceding_params.yaml + :language: yaml diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index f7b8cc4491..ce66fd06d4 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -28,17 +28,11 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "pid_controller/visibility_control.h" #include "pid_controller_parameters.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" -#include "control_msgs/msg/joint_controller_state.hpp" - -#include "control_msgs/msg/pid_state.hpp" -#include "trajectory_msgs/msg/joint_trajectory_point.hpp" - namespace pid_controller { @@ -123,6 +117,8 @@ class PidController : public controller_interface::ChainableControllerInterface // override methods from ChainableControllerInterface std::vector on_export_reference_interfaces() override; + std::vector on_export_state_interfaces() override; + bool on_set_chained_mode(bool chained_mode) override; // internal methods diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 9cda85aa03..79a66d3cc3 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.6.0 + 4.15.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl @@ -14,6 +14,7 @@ generate_parameter_library angles + backward_ros control_msgs control_toolbox controller_interface diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 05fee986dd..032dc1d666 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -24,16 +24,30 @@ #include "angles/angles.h" #include "control_msgs/msg/single_dof_state.hpp" -#include "controller_interface/helpers.hpp" +#include "rclcpp/version.h" namespace { // utility // Changed services history QoS to keep all so we don't lose any client service calls +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCLCPP_VERSION_MAJOR >= 17 rclcpp::QoS qos_services = rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) .reliable() .durability_volatile(); +#else +static const rmw_qos_profile_t qos_services = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; +#endif using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; @@ -173,6 +187,43 @@ controller_interface::CallbackReturn PidController::on_configure( auto measured_state_callback = [&](const std::shared_ptr state_msg) -> void { + if (state_msg->dof_names.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data names (%zu) is not matching the expected size (%zu).", + state_msg->dof_names.size(), reference_and_state_dof_names_.size()); + return; + } + if (state_msg->values.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data values (%zu) is not matching the expected size (%zu).", + state_msg->values.size(), reference_and_state_dof_names_.size()); + return; + } + + if (!state_msg->values_dot.empty()) + { + if (params_.reference_and_state_interfaces.size() != 2) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "The reference_and_state_interfaces parameter has to have two interfaces [the " + "interface and the derivative of the interface], in order to use the values_dot " + "field."); + return; + } + if (state_msg->values_dot.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data values_dot (%zu) is not matching the expected size (%zu).", + state_msg->values_dot.size(), reference_and_state_dof_names_.size()); + return; + } + } // TODO(destogl): Sort the input values based on joint and interface names measured_state_.writeFromNonRT(state_msg); }; @@ -349,6 +400,27 @@ std::vector PidController::on_export_refer return reference_interfaces; } +std::vector PidController::on_export_state_interfaces() +{ + std::vector state_interfaces; + state_interfaces.reserve(state_interfaces_values_.size()); + + state_interfaces_values_.resize( + reference_and_state_dof_names_.size() * params_.reference_and_state_interfaces.size(), + std::numeric_limits::quiet_NaN()); + size_t index = 0; + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + state_interfaces.push_back(hardware_interface::StateInterface( + get_node()->get_name(), dof_name + "/" + interface, &state_interfaces_values_[index])); + ++index; + } + } + return state_interfaces; +} + bool PidController::on_set_chained_mode(bool chained_mode) { // Always accept switch to/from chained mode @@ -424,6 +496,12 @@ controller_interface::return_type PidController::update_and_write_commands( } } + // Fill the information of the exported state interfaces + for (size_t i = 0; i < measured_state_values_.size(); ++i) + { + state_interfaces_values_[i] = measured_state_values_[i]; + } + for (size_t i = 0; i < dof_; ++i) { double tmp_command = std::numeric_limits::quiet_NaN(); diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index f645738862..651cc1e7de 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -34,6 +34,7 @@ pid_controller: read_only: true, validation: { not_empty<>: null, + size_gt<>: 0, size_lt<>: 3, } } diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp index 3a75f6e170..5cfb04e860 100644 --- a/pid_controller/test/test_load_pid_controller.cpp +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -33,9 +33,7 @@ TEST(TestLoadPidController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller("test_pid_controller", "pid_controller/PidController")); diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index a44347f5f1..60d484e463 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include using pid_controller::feedforward_mode_type; @@ -98,9 +97,9 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + 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; } } @@ -495,7 +494,7 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) } publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) { diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 1c356263e7..4471f35a12 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -18,20 +18,19 @@ #ifndef TEST_PID_CONTROLLER_HPP_ #define TEST_PID_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "pid_controller/pid_controller.hpp" -#include "rclcpp/parameter_value.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -65,19 +64,14 @@ class TestablePidController : public pid_controller::PidController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = pid_controller::PidController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_); - } - return ret; + return pid_controller::PidController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override { auto ref_itfs = on_export_reference_interfaces(); + auto state_itfs = on_export_state_interfaces(); return pid_controller::PidController::on_activate(previous_state); } @@ -85,30 +79,25 @@ class TestablePidController : public pid_controller::PidController * @brief wait_for_command blocks until a new ControllerCommandMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerCommandMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -182,36 +171,43 @@ class PidControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_pid_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands( diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 3e17e69286..ee5fe46754 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -17,10 +17,8 @@ #include "test_pid_controller.hpp" -#include #include #include -#include #include using pid_controller::feedforward_mode_type; @@ -86,12 +84,30 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + 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; } } + + // check exported state itfs + auto exported_state_itfs = controller_->export_state_interfaces(); + ASSERT_EQ(exported_state_itfs.size(), dof_state_values_.size()); + size_t esi_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + const std::string state_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(exported_state_itfs[esi_index]->get_name(), state_itf_name); + EXPECT_EQ( + exported_state_itfs[esi_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(exported_state_itfs[esi_index]->get_interface_name(), dof_name + "/" + interface); + ++esi_index; + } + } } int main(int argc, char ** argv) diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..46028cf258 --- /dev/null +++ b/pose_broadcaster/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.16) +project(pose_broadcaster + LANGUAGES + CXX +) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(pose_broadcaster_parameters + src/pose_broadcaster_parameters.yaml +) + +add_library(pose_broadcaster SHARED + src/pose_broadcaster.cpp +) +target_compile_features(pose_broadcaster PUBLIC + cxx_std_17 +) +target_include_directories(pose_broadcaster PUBLIC + $ + $ +) +target_link_libraries(pose_broadcaster PUBLIC + pose_broadcaster_parameters +) +ament_target_dependencies(pose_broadcaster PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pose_broadcaster PRIVATE "POSE_BROADCASTER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface pose_broadcaster.xml +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_pose_broadcaster + test/test_load_pose_broadcaster.cpp + ) + target_link_libraries(test_load_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_load_pose_broadcaster + controller_manager + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_pose_broadcaster + test/test_pose_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pose_broadcaster_params.yaml + ) + target_link_libraries(test_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_pose_broadcaster + hardware_interface + ) +endif() + +install( + DIRECTORY + include/ + DESTINATION include/${PROJECT_NAME} +) +install( + TARGETS + pose_broadcaster + pose_broadcaster_parameters + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/pose_broadcaster/README.md b/pose_broadcaster/README.md new file mode 100644 index 0000000000..bf47411048 --- /dev/null +++ b/pose_broadcaster/README.md @@ -0,0 +1,8 @@ +pose_broadcaster +========================================== + +Controller to publish poses provided by pose sensors. + +Pluginlib-Library: pose_broadcaster + +Plugin: pose_broadcaster/PoseBroadcaster (controller_interface::ControllerInterface) diff --git a/pose_broadcaster/doc/userdoc.rst b/pose_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..0ae40e2fad --- /dev/null +++ b/pose_broadcaster/doc/userdoc.rst @@ -0,0 +1,27 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pose_broadcaster/doc/userdoc.rst + +.. _pose_broadcaster_userdoc: + +Pose Broadcaster +-------------------------------- +Broadcaster for poses measured by a robot or a sensor. +Poses are published as ``geometry_msgs/msg/PoseStamped`` messages and optionally as tf transforms. + +The controller is a wrapper around the ``PoseSensor`` semantic component (see ``controller_interface`` package). + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/pose_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/pose_broadcaster_params.yaml + :language: yaml diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp new file mode 100644 index 0000000000..621a90cc85 --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef POSE_BROADCASTER__POSE_BROADCASTER_HPP_ +#define POSE_BROADCASTER__POSE_BROADCASTER_HPP_ + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "pose_broadcaster/visibility_control.h" +#include "pose_broadcaster_parameters.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_publisher.h" +#include "semantic_components/pose_sensor.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +namespace pose_broadcaster +{ + +class PoseBroadcaster : public controller_interface::ControllerInterface +{ +public: + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + std::shared_ptr param_listener_; + Params params_; + + std::unique_ptr pose_sensor_; + + rclcpp::Publisher::SharedPtr pose_publisher_; + std::unique_ptr> + realtime_publisher_; + + std::optional tf_publish_period_; + rclcpp::Time tf_last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; + rclcpp::Publisher::SharedPtr tf_publisher_; + std::unique_ptr> + realtime_tf_publisher_; +}; + +} // namespace pose_broadcaster + +#endif // POSE_BROADCASTER__POSE_BROADCASTER_HPP_ diff --git a/pose_broadcaster/include/pose_broadcaster/visibility_control.h b/pose_broadcaster/include/pose_broadcaster/visibility_control.h new file mode 100644 index 0000000000..5ce272658d --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_BROADCASTER__VISIBILITY_CONTROL_H_ +#define POSE_BROADCASTER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define POSE_BROADCASTER_EXPORT __attribute__((dllexport)) +#define POSE_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define POSE_BROADCASTER_EXPORT __declspec(dllexport) +#define POSE_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef POSE_BROADCASTER_BUILDING_DLL +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_EXPORT +#else +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_IMPORT +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#else +#define POSE_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define POSE_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define POSE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define POSE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // POSE_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml new file mode 100644 index 0000000000..4c3506f1cd --- /dev/null +++ b/pose_broadcaster/package.xml @@ -0,0 +1,31 @@ + + + + pose_broadcaster + 0.0.0 + Broadcaster to publish cartesian states. + Robert Wilbrandt + + Apache License 2.0 + + ament_cmake + + backward_ros + controller_interface + generate_parameter_library + geometry_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2_msgs + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/pose_broadcaster/pose_broadcaster.xml b/pose_broadcaster/pose_broadcaster.xml new file mode 100644 index 0000000000..6578958004 --- /dev/null +++ b/pose_broadcaster/pose_broadcaster.xml @@ -0,0 +1,9 @@ + + + + This controller publishes a Cartesian state as a geometry_msgs/PoseStamped message and optionally as a tf transform. + + + diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp new file mode 100644 index 0000000000..7e3aeaddf3 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -0,0 +1,195 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "pose_broadcaster/pose_broadcaster.hpp" + +namespace +{ + +constexpr auto DEFAULT_POSE_TOPIC = "~/pose"; +constexpr auto DEFAULT_TF_TOPIC = "/tf"; + +} // namespace + +namespace pose_broadcaster +{ + +controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = pose_sensor_->get_state_interface_names(); + + return state_interfaces_config; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & ex) + { + fprintf(stderr, "Exception thrown during init stage with message: %s\n", ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + pose_sensor_ = std::make_unique(params_.pose_name); + tf_publish_period_ = + params_.tf.publish_rate == 0.0 + ? std::nullopt + : std::optional{rclcpp::Duration::from_seconds(1.0 / params_.tf.publish_rate)}; + + try + { + pose_publisher_ = get_node()->create_publisher( + DEFAULT_POSE_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = + std::make_unique>( + pose_publisher_); + + if (params_.tf.enable) + { + tf_publisher_ = get_node()->create_publisher( + DEFAULT_TF_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_tf_publisher_ = + std::make_unique>( + tf_publisher_); + } + } + catch (const std::exception & ex) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message: %s\n", + ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Initialize pose message + realtime_publisher_->lock(); + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_publisher_->unlock(); + + // Initialize tf message if tf publishing is enabled + if (realtime_tf_publisher_) + { + realtime_tf_publisher_->lock(); + + realtime_tf_publisher_->msg_.transforms.resize(1); + auto & tf_transform = realtime_tf_publisher_->msg_.transforms.front(); + tf_transform.header.frame_id = params_.frame_id; + if (params_.tf.child_frame_id.empty()) + { + tf_transform.child_frame_id = params_.pose_name; + } + else + { + tf_transform.child_frame_id = params_.tf.child_frame_id; + } + + realtime_tf_publisher_->unlock(); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + pose_sensor_->assign_loaned_state_interfaces(state_interfaces_); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + pose_sensor_->release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PoseBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + geometry_msgs::msg::Pose pose; + pose_sensor_->get_values_as_message(pose); + + if (realtime_publisher_ && realtime_publisher_->trylock()) + { + realtime_publisher_->msg_.header.stamp = time; + realtime_publisher_->msg_.pose = pose; + realtime_publisher_->unlockAndPublish(); + } + + if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) + { + bool do_publish = false; + // rlcpp::Time comparisons throw if clock types are not the same + if (tf_last_publish_time_.get_clock_type() != time.get_clock_type()) + { + do_publish = true; + } + else if (!tf_publish_period_ || (tf_last_publish_time_ + *tf_publish_period_ <= time)) + { + do_publish = true; + } + + if (do_publish) + { + auto & tf_transform = realtime_tf_publisher_->msg_.transforms[0]; + tf_transform.header.stamp = time; + + tf_transform.transform.translation.x = pose.position.x; + tf_transform.transform.translation.y = pose.position.y; + tf_transform.transform.translation.z = pose.position.z; + + tf_transform.transform.rotation.x = pose.orientation.x; + tf_transform.transform.rotation.y = pose.orientation.y; + tf_transform.transform.rotation.z = pose.orientation.z; + tf_transform.transform.rotation.w = pose.orientation.w; + + realtime_tf_publisher_->unlockAndPublish(); + + tf_last_publish_time_ = time; + } + else + { + realtime_tf_publisher_->unlock(); + } + } + + return controller_interface::return_type::OK; +} + +} // namespace pose_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(pose_broadcaster::PoseBroadcaster, controller_interface::ControllerInterface) diff --git a/pose_broadcaster/src/pose_broadcaster_parameters.yaml b/pose_broadcaster/src/pose_broadcaster_parameters.yaml new file mode 100644 index 0000000000..11c53b5e57 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster_parameters.yaml @@ -0,0 +1,32 @@ +pose_broadcaster: + frame_id: + type: string + default_value: "" + description: "frame_id in which values are published" + validation: + not_empty<>: null + pose_name: + type: string + default_value: "" + description: "Base name used as prefix for controller interfaces. + The state interface names are: ``/position.x, ..., /position.z, + /orientation.x, ..., /orientation.w``" + validation: + not_empty<>: null + tf: + enable: + type: bool + default_value: true + description: "Whether to publish the pose as a tf transform" + child_frame_id: + type: string + default_value: "" + description: "Child frame id of published tf transforms. Defaults to ``pose_name`` if left + empty." + publish_rate: + type: double + default_value: 0.0 + description: "Rate to limit publishing of tf transforms to (Hz). If set to 0, no limiting is + performed." + validation: + gt_eq<>: 0.0 diff --git a/pose_broadcaster/test/pose_broadcaster_params.yaml b/pose_broadcaster/test/pose_broadcaster_params.yaml new file mode 100644 index 0000000000..a2f8477dd1 --- /dev/null +++ b/pose_broadcaster/test/pose_broadcaster_params.yaml @@ -0,0 +1,4 @@ +test_pose_broadcaster: + ros__parameters: + pose_name: "test_pose" + frame_id: "pose_frame" diff --git a/pose_broadcaster/test/test_load_pose_broadcaster.cpp b/pose_broadcaster/test/test_load_pose_broadcaster.cpp new file mode 100644 index 0000000000..bdf72d7b23 --- /dev/null +++ b/pose_broadcaster/test/test_load_pose_broadcaster.cpp @@ -0,0 +1,50 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadPoseBroadcaster, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm{ + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"}; + + const std::string test_file_path = + std::string{TEST_FILES_DIRECTORY} + "/pose_broadcaster_params.yaml"; + cm.set_parameter({"test_pose_broadcaster.params_file", test_file_path}); + + cm.set_parameter({"test_pose_broadcaster.type", "pose_broadcaster/PoseBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_pose_broadcaster"), nullptr); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp new file mode 100644 index 0000000000..0ed2e84619 --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -0,0 +1,198 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "test_pose_broadcaster.hpp" + +#include +#include + +using hardware_interface::LoanedStateInterface; + +void PoseBroadcasterTest::SetUp() { pose_broadcaster_ = std::make_unique(); } + +void PoseBroadcasterTest::TearDown() { pose_broadcaster_.reset(nullptr); } + +void PoseBroadcasterTest::SetUpPoseBroadcaster() +{ + ASSERT_EQ( + pose_broadcaster_->init( + "test_pose_broadcaster", "", 0, "", pose_broadcaster_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector state_interfaces; + state_interfaces.emplace_back(pose_position_x_); + state_interfaces.emplace_back(pose_position_y_); + state_interfaces.emplace_back(pose_position_z_); + state_interfaces.emplace_back(pose_orientation_x_); + state_interfaces.emplace_back(pose_orientation_y_); + state_interfaces.emplace_back(pose_orientation_z_); + state_interfaces.emplace_back(pose_orientation_w_); + + pose_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); +} + +TEST_F(PoseBroadcasterTest, Configure_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command interface configuration + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ(command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + // Verify state interface configuration + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); +} + +TEST_F(PoseBroadcasterTest, Activate_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); + } + + // Deactivate controller + ASSERT_EQ( + pose_broadcaster_->on_deactivate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); // Should not change when deactivating + } +} + +TEST_F(PoseBroadcasterTest, Update_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + ASSERT_EQ( + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PoseBroadcasterTest, PublishSuccess) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Set 'tf.enable' and 'tf.child_frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"tf.enable", true}); + pose_broadcaster_->get_node()->set_parameter({"tf.child_frame_id", tf_child_frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Subscribe to pose topic + geometry_msgs::msg::PoseStamped pose_msg; + subscribe_and_get_message("/test_pose_broadcaster/pose", pose_msg); + + // Verify content of pose message + EXPECT_EQ(pose_msg.header.frame_id, frame_id_); + EXPECT_EQ(pose_msg.pose.position.x, pose_values_[0]); + EXPECT_EQ(pose_msg.pose.position.y, pose_values_[1]); + EXPECT_EQ(pose_msg.pose.position.z, pose_values_[2]); + EXPECT_EQ(pose_msg.pose.orientation.x, pose_values_[3]); + EXPECT_EQ(pose_msg.pose.orientation.y, pose_values_[4]); + EXPECT_EQ(pose_msg.pose.orientation.z, pose_values_[5]); + EXPECT_EQ(pose_msg.pose.orientation.w, pose_values_[6]); + + // Subscribe to tf topic + tf2_msgs::msg::TFMessage tf_msg; + subscribe_and_get_message("/tf", tf_msg); + + // Verify content of tf message + ASSERT_EQ(tf_msg.transforms.size(), 1lu); + EXPECT_EQ(tf_msg.transforms[0].header.frame_id, frame_id_); + EXPECT_EQ(tf_msg.transforms[0].child_frame_id, tf_child_frame_id_); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.x, pose_values_[0]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.y, pose_values_[1]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.z, pose_values_[2]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.x, pose_values_[3]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.y, pose_values_[4]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.z, pose_values_[5]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.w, pose_values_[6]); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp new file mode 100644 index 0000000000..a164b2c6ac --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -0,0 +1,96 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef TEST_POSE_BROADCASTER_HPP_ +#define TEST_POSE_BROADCASTER_HPP_ + +#include + +#include +#include +#include + +#include "rclcpp/executors.hpp" + +#include "pose_broadcaster/pose_broadcaster.hpp" + +using pose_broadcaster::PoseBroadcaster; + +class PoseBroadcasterTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + + void SetUpPoseBroadcaster(); + +protected: + const std::string pose_name_ = "test_pose"; + const std::string frame_id_ = "pose_base_frame"; + const std::string tf_child_frame_id_ = "pose_frame"; + + std::array pose_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}; + + hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; + hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; + hardware_interface::StateInterface pose_position_z_{pose_name_, "position.x", &pose_values_[2]}; + hardware_interface::StateInterface pose_orientation_x_{ + pose_name_, "orientation.x", &pose_values_[3]}; + hardware_interface::StateInterface pose_orientation_y_{ + pose_name_, "orientation.y", &pose_values_[4]}; + hardware_interface::StateInterface pose_orientation_z_{ + pose_name_, "orientation.z", &pose_values_[5]}; + hardware_interface::StateInterface pose_orientation_w_{ + pose_name_, "orientation.w", &pose_values_[6]}; + + std::unique_ptr pose_broadcaster_; + + template + void subscribe_and_get_message(const std::string & topic, T & msg); +}; + +template +void PoseBroadcasterTest::subscribe_and_get_message(const std::string & topic, T & msg) +{ + // Create node for subscribing + rclcpp::Node node{"test_subscription_node"}; + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node.get_node_base_interface()); + + // Create subscription + typename T::SharedPtr received_msg; + const auto msg_callback = [&](const typename T::SharedPtr sub_msg) { received_msg = sub_msg; }; + const auto subscription = node.create_subscription(topic, 10, msg_callback); + + // Update controller and spin until a message is received + // Since update doesn't guarantee a published message, republish until received + constexpr size_t max_sub_check_loop_count = 5; + for (size_t i = 0; !received_msg; ++i) + { + ASSERT_LT(i, max_sub_check_loop_count); + + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)); + + const auto timeout = std::chrono::milliseconds{5}; + const auto until = node.get_clock()->now() + timeout; + while (!received_msg && node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds{10}); + } + } + + msg = *received_msg; +} + +#endif // TEST_POSE_BROADCASTER_HPP_ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 6c55f59602..d52201f2b4 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,42 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.hpp b/position_controllers/include/position_controllers/joint_group_position_controller.hpp index 47705b6a3d..4eaf9086e4 100644 --- a/position_controllers/include/position_controllers/joint_group_position_controller.hpp +++ b/position_controllers/include/position_controllers/joint_group_position_controller.hpp @@ -15,11 +15,8 @@ #ifndef POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ #define POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ -#include - #include "forward_command_controller/forward_command_controller.hpp" #include "position_controllers/visibility_control.h" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace position_controllers { diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 13e80f4d99..c03888eaf8 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.6.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp index 8335a200c4..1074ba7ef7 100644 --- a/position_controllers/src/joint_group_position_controller.cpp +++ b/position_controllers/src/joint_group_position_controller.cpp @@ -17,7 +17,6 @@ #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" -#include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" namespace position_controllers diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 60bff556db..96a5cead17 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -12,35 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_position_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupPositionControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupPositionControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +47,7 @@ void JointGroupPositionControllerTest::SetUpController() command_ifs.emplace_back(joint_2_pos_cmd_); command_ifs.emplace_back(joint_3_pos_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupPositionControllerTest, JointsParameterNotSet) @@ -204,10 +189,13 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index 93149d8e19..fefd2073e3 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -15,15 +15,16 @@ #ifndef TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; @@ -58,6 +59,7 @@ class JointGroupPositionControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp index bc27b5e629..f71474bd4a 100644 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ b/position_controllers/test/test_load_joint_group_position_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupPositionController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller( diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 62c87c8717..77eae62d2a 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,48 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ +* added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Contributors: Sai Kishor Kothakota + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt index d7002d1b17..6f21607c9c 100644 --- a/range_sensor_broadcaster/CMakeLists.txt +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -59,9 +59,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_range_sensor_broadcaster - test/test_load_range_sensor_broadcaster.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/range_sensor_broadcaster_params.yaml) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_range_sensor_broadcaster test/test_load_range_sensor_broadcaster.cpp) target_include_directories(test_load_range_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_range_sensor_broadcaster range_sensor_broadcaster diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp index b2e5fbfac0..5a93f95982 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -20,13 +20,10 @@ #define RANGE_SENSOR_BROADCASTER__RANGE_SENSOR_BROADCASTER_HPP_ #include -#include -#include #include "controller_interface/controller_interface.hpp" #include "range_sensor_broadcaster/visibility_control.h" #include "range_sensor_broadcaster_parameters.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/range_sensor.hpp" diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 3bbe1d2130..92283fe3ad 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.6.0 + 4.15.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp index b821da8c13..7c6d714be3 100644 --- a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp @@ -79,7 +79,10 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure( realtime_publisher_->msg_.field_of_view = params_.field_of_view; realtime_publisher_->msg_.min_range = params_.min_range; realtime_publisher_->msg_.max_range = params_.max_range; +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if SENSOR_MSGS_VERSION_MAJOR >= 5 realtime_publisher_->msg_.variance = params_.variance; +#endif realtime_publisher_->unlock(); RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp index 5c400bef91..11aae67f5d 100644 --- a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -32,14 +32,15 @@ TEST(TestLoadRangeSensorBroadcaster, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); - - ASSERT_NE( - cm.load_controller( - "test_range_sensor_broadcaster", "range_sensor_broadcaster/RangeSensorBroadcaster"), - nullptr); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/range_sensor_broadcaster_params.yaml"; + + cm.set_parameter({"test_range_sensor_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_range_sensor_broadcaster.type", "range_sensor_broadcaster/RangeSensorBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_range_sensor_broadcaster"), nullptr); } int main(int argc, char ** argv) diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 010f18c1a6..59d27ebc0c 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -21,6 +21,8 @@ #include "test_range_sensor_broadcaster.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" using testing::IsEmpty; using testing::SizeIs; @@ -66,31 +68,39 @@ controller_interface::CallbackReturn RangeSensorBroadcasterTest::configure_broad void RangeSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Range & range_msg) { // create a new subscriber + sensor_msgs::msg::Range::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const sensor_msgs::msg::Range::SharedPtr) {}; + auto subs_callback = [&](const sensor_msgs::msg::Range::SharedPtr msg) { received_msg = msg; }; auto subscription = test_subscription_node.create_subscription( "/test_range_sensor_broadcaster/range", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { range_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(range_msg, msg_info)); + range_msg = *received_msg; } TEST_F(RangeSensorBroadcasterTest, Initialize_RangeBroadcaster_Exception) @@ -203,7 +213,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_RangeBroadcaster_Success) EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +#if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +#endif } TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) @@ -224,7 +236,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +#if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +#endif sensor_range_ = 4.0; subscribe_and_get_message(range_msg); @@ -235,7 +249,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +#if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +#endif } TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Success) @@ -257,7 +273,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +#if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +#endif sensor_range_ = 6.0; subscribe_and_get_message(range_msg); @@ -269,7 +287,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_)); +#if SENSOR_MSGS_VERSION_MAJOR >= 5 EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); +#endif } int main(int argc, char ** argv) diff --git a/ros2_controllers-not-released.jazzy.repos b/ros2_controllers-not-released.jazzy.repos new file mode 100644 index 0000000000..66352f4960 --- /dev/null +++ b/ros2_controllers-not-released.jazzy.repos @@ -0,0 +1,5 @@ +repositories: + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main diff --git a/ros2_controllers.jazzy.repos b/ros2_controllers.jazzy.repos new file mode 100644 index 0000000000..8c20eccc96 --- /dev/null +++ b/ros2_controllers.jazzy.repos @@ -0,0 +1,25 @@ +repositories: + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: master + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: master + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master + kinematics_interface: + type: git + url: https://github.com/ros-controls/kinematics_interface.git + version: master + generate_parameter_library: + type: git + url: https://github.com/picknikrobotics/generate_parameter_library.git + version: main diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index fc8ca62020..bd9e0a8b38 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,38 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ +* Add custom rosdoc2 config for ros2_controllers metapackage (`#1100 `_) +* Contributors: Christoph Fröhlich + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + 4.6.0 (2024-02-12) ------------------ diff --git a/ros2_controllers/doc/conf.py b/ros2_controllers/doc/conf.py new file mode 100644 index 0000000000..b6134e9abd --- /dev/null +++ b/ros2_controllers/doc/conf.py @@ -0,0 +1,5 @@ +# Configuration file for the Sphinx documentation builder. +# settings will be overridden by rosdoc2, so we add here only custom settings + +copyright = "2024, ros2_control development team" +html_logo = "https://control.ros.org/master/_static/logo_ros-controls.png" diff --git a/ros2_controllers/doc/index.rst b/ros2_controllers/doc/index.rst new file mode 100644 index 0000000000..f785112944 --- /dev/null +++ b/ros2_controllers/doc/index.rst @@ -0,0 +1,80 @@ +Welcome to the documentation for ros2_controllers +================================================= + +For more information of the ros2_control framework see `control.ros.org `__. + +.. list-table:: + :header-rows: 1 + + * - Package Name + - Documentation + - API + - ROS Index + * - ackermann_steering_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - admittance_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - bicycle_steering_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - diff_drive_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - effort_controllers + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - force_torque_sensor_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - forward_command_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - imu_sensor_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - joint_state_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - joint_trajectory_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - pid_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - position_controllers + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - range_sensor_broadcaster + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - steering_controllers_library + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - tricycle_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - tricycle_steering_controller + - `Documentation `__ + - `API `__ + - `ROS Index `__ + * - velocity_controllers + - `Documentation `__ + - `API `__ + - `ROS Index `__ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 10ae5427e9..0886b489c7 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,13 +1,15 @@ ros2_controllers - 4.6.0 + 4.15.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios Apache License 2.0 + https://control.ros.org + ament_cmake ackermann_steering_controller diff --git a/ros2_controllers/rosdoc2.yaml b/ros2_controllers/rosdoc2.yaml new file mode 100644 index 0000000000..c5ec1b53a0 --- /dev/null +++ b/ros2_controllers/rosdoc2.yaml @@ -0,0 +1,20 @@ +type: 'rosdoc2 config' +version: 1 + +--- + +settings: + # Not generating any documentation of code + generate_package_index: false + always_run_doxygen: false + enable_breathe: false + enable_exhale: false + always_run_sphinx_apidoc: false + +builders: + # Configure Sphinx with the location of the docs: + - sphinx: { + name: 'ros2_controllers', + sphinx_sourcedir: 'doc', + output_dir: '' + } diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 41a20f3841..29ca6cd84d 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,38 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* Fix deprecation warning in paramater declaration (`#1280 `_) +* Contributors: Sanjeev + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + 4.6.0 (2024-02-12) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index fb5b3d8bcf..f61a51e34e 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.6.0 + 4.15.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_forward_position_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py index 5cf28ac604..bb6add77ef 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py @@ -37,7 +37,7 @@ def __init__(self): # Read all positions from parameters self.goals = [] for name in goal_names: - self.declare_parameter(name) + self.declare_parameter(name, rclpy.Parameter.Type.DOUBLE_ARRAY) goal = self.get_parameter(name).value if goal is None or len(goal) == 0: raise Exception(f'Values for goal "{name}" not set!') 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..27f28da1be 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 @@ -18,7 +18,6 @@ import rclpy from rclpy.node import Node from builtin_interfaces.msg import Duration -from rcl_interfaces.msg import ParameterDescriptor from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from sensor_msgs.msg import JointState @@ -67,8 +66,7 @@ def __init__(self): # Read all positions from parameters self.goals = [] # List of JointTrajectoryPoint for name in goal_names: - self.declare_parameter(name, descriptor=ParameterDescriptor(dynamic_typing=True)) - + self.declare_parameter(name, rclpy.Parameter.Type.DOUBLE_ARRAY) point = JointTrajectoryPoint() def get_sub_param(sub_param): diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 9219fea906..59aee66875 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.6.0", + version="4.15.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 cd4ae0809e..043930c536 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,42 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* Fix bug for displaying all controllers (`#1259 `_) +* Contributors: Francisco Martín Rico + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ +* [RQT-JTC] limits from jtc controlled joints (`#1146 `_) +* Contributors: Jakub Delicat + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ +* [CI] Code coverage + pre-commit (`#1057 `_) +* Contributors: Christoph Fröhlich + 4.6.0 (2024-02-12) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 107d50681c..7920e63246 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.6.0 + 4.15.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py index 5655e12f7a..bdde80dd54 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -45,7 +45,7 @@ def subscribe_to_robot_description(node, key="robot_description"): node.create_subscription(String, key, callback, qos_profile) -def get_joint_limits(node, use_smallest_joint_limits=True): +def get_joint_limits(node, joints_names, use_smallest_joint_limits=True): global description use_small = use_smallest_joint_limits use_mimic = True @@ -71,54 +71,56 @@ def get_joint_limits(node, use_smallest_joint_limits=True): if jtype == "fixed": continue name = child.getAttribute("name") - try: - limit = child.getElementsByTagName("limit")[0] + + if name in joints_names: try: - minval = float(limit.getAttribute("lower")) - maxval = float(limit.getAttribute("upper")) - except ValueError: - if jtype == "continuous": - minval = -pi - maxval = pi - else: + limit = child.getElementsByTagName("limit")[0] + try: + minval = float(limit.getAttribute("lower")) + maxval = float(limit.getAttribute("upper")) + except ValueError: + if jtype == "continuous": + minval = -pi + maxval = pi + else: + raise Exception( + f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + ) + try: + maxvel = float(limit.getAttribute("velocity")) + except ValueError: raise Exception( - f"Missing lower/upper position limits for the joint : {name} of type : {jtype} in the robot_description!" + f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" ) - try: - maxvel = float(limit.getAttribute("velocity")) - except ValueError: + except IndexError: raise Exception( - f"Missing velocity limits for the joint : {name} of type : {jtype} in the robot_description!" + f"Missing limits tag for the joint : {name} in the robot_description!" ) - except IndexError: - raise Exception( - f"Missing limits tag for the joint : {name} in the robot_description!" - ) - safety_tags = child.getElementsByTagName("safety_controller") - if use_small and len(safety_tags) == 1: - tag = safety_tags[0] - if tag.hasAttribute("soft_lower_limit"): - minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) - if tag.hasAttribute("soft_upper_limit"): - maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) - - mimic_tags = child.getElementsByTagName("mimic") - if use_mimic and len(mimic_tags) == 1: - tag = mimic_tags[0] - entry = {"parent": tag.getAttribute("joint")} - if tag.hasAttribute("multiplier"): - entry["factor"] = float(tag.getAttribute("multiplier")) - if tag.hasAttribute("offset"): - entry["offset"] = float(tag.getAttribute("offset")) - - dependent_joints[name] = entry - continue - - if name in dependent_joints: - continue - - joint = {"min_position": minval, "max_position": maxval} - joint["has_position_limits"] = jtype != "continuous" - joint["max_velocity"] = maxvel - free_joints[name] = joint + safety_tags = child.getElementsByTagName("safety_controller") + if use_small and len(safety_tags) == 1: + tag = safety_tags[0] + if tag.hasAttribute("soft_lower_limit"): + minval = max(minval, float(tag.getAttribute("soft_lower_limit"))) + if tag.hasAttribute("soft_upper_limit"): + maxval = min(maxval, float(tag.getAttribute("soft_upper_limit"))) + + mimic_tags = child.getElementsByTagName("mimic") + if use_mimic and len(mimic_tags) == 1: + tag = mimic_tags[0] + entry = {"parent": tag.getAttribute("joint")} + if tag.hasAttribute("multiplier"): + entry["factor"] = float(tag.getAttribute("multiplier")) + if tag.hasAttribute("offset"): + entry["offset"] = float(tag.getAttribute("offset")) + + dependent_joints[name] = entry + continue + + if name in dependent_joints: + continue + + joint = {"min_position": minval, "max_position": maxval} + joint["has_position_limits"] = jtype != "continuous" + joint["max_velocity"] = maxvel + free_joints[name] = joint return free_joints diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 162977cdfe..5b27c2c832 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -214,9 +214,9 @@ def restore_settings(self, plugin_settings, instance_settings): try: idx = jtc_list.index(jtc_name) jtc_combo.setCurrentIndex(idx) - except (ValueError): + except ValueError: pass - except (ValueError): + except ValueError: pass # def trigger_configuration(self): @@ -238,7 +238,11 @@ def _update_jtc_list(self): # for _all_ their joints running_jtc = self._running_jtc_info() if running_jtc and not self._robot_joint_limits: - self._robot_joint_limits = get_joint_limits(self._node) # Lazy evaluation + self._robot_joint_limits = {} + for jtc_info in running_jtc: + self._robot_joint_limits.update( + get_joint_limits(self._node, _jtc_joint_names(jtc_info)) + ) valid_jtc = [] if self._robot_joint_limits: for jtc_info in running_jtc: @@ -427,7 +431,7 @@ def _update_cmd_cb(self): cmd = pos try: cmd = self._joint_pos[name]["command"] - except (KeyError): + except KeyError: pass max_vel = self._robot_joint_limits[name]["max_velocity"] dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) @@ -445,7 +449,7 @@ def _update_joint_widgets(self): try: joint_pos = self._joint_pos[joint_name]["position"] joint_widgets[id].setValue(joint_pos) - except (KeyError): + except KeyError: pass # Can happen when first connected to controller def _joint_widgets(self): # TODO: Cache instead of compute every time? diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py index f0b632322f..6f736910e2 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -36,7 +36,7 @@ def update_combo(combo, new_vals): selected_id = -1 try: selected_id = new_vals.index(selected_val) - except (ValueError): + except ValueError: combo.setCurrentIndex(-1) # Re-populate items diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 8bcb07ac54..a70342e154 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -1,5 +1,19 @@ #!/usr/bin/env python +# Copyright 2024 Apache License, Version 2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + from setuptools import setup from glob import glob @@ -7,7 +21,7 @@ setup( name=package_name, - version="4.6.0", + version="4.15.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 e8288f1955..39360d6921 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,63 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* fix(timeout): do not reset steer wheels to 0. on timeout (`#1289 `_) +* fix(steering-odometry): convert twist to steering angle (`#1288 `_) +* Contributors: Manuel Muth, Rein Appeldoorn + +4.14.0 (2024-09-11) +------------------- +* fix(steering-odometry): handle infinite turning radius properly (`#1285 `_) +* Contributors: Rein Appeldoorn + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich + +4.10.0 (2024-07-01) +------------------- +* [STEERING] Add missing `tan` call for ackermann (`#1117 `_) +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Enrique Llorente Pastora, Quique Llorente + +4.9.0 (2024-06-05) +------------------ +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) +* Fix correct usage of angular velocity in update_odometry() function (`#1118 `_) +* Contributors: Christoph Fröhlich, Ferry Schoenmakers + +4.8.0 (2024-05-14) +------------------ +* Deprecate non-stamped twist for tricycle_controller and steering_controllers (`#1093 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.7.0 (2024-03-22) +------------------ +* added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Sai Kishor Kothakota, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Fix usage of M_PI on Windows (`#1036 `_) diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index b79236e23e..e2bfdbab71 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -30,6 +30,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(steering_controllers_library_parameters src/steering_controllers_library.yaml diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index df3d1529d0..44b180162e 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -5,17 +5,25 @@ steering_controllers_library ============================= -Library with shared functionalities for mobile robot controllers with steering drive (2 degrees of freedom). +.. _steering_controller_status_msg: https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/SteeringControllerStatus.msg +.. _odometry_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/nav_msgs/msg/Odometry.msg +.. _twist_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/geometry_msgs/msg/TwistStamped.msg +.. _tf_msg: https://github.com/ros2/geometry2/blob/{DISTRO}/tf2_msgs/msg/TFMessage.msg + +Library with shared functionalities for mobile robot controllers with steering drives, with so-called non-holonomic constraints. + The library implements generic odometry and update methods and defines the main interfaces. -Nomenclature used for the controller is used from `wikipedia `_. +The update methods only use inverse kinematics, it does not implement any feedback control loops like path-tracking controllers etc. + +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. Execution logic of the controller ---------------------------------- -The controller uses velocity input, i.e., stamped or unstamped Twist messages where linear ``x`` and angular ``z`` components are used. -Angular component under +The controller uses velocity input, i.e., stamped `twist messages `_ where linear ``x`` and angular ``z`` components are used. Values in other components are ignored. + In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position. Other relevant features are: @@ -24,68 +32,82 @@ Other relevant features are: * input command timeout based on a parameter. The command for the wheels are calculated using ``odometry`` library where based on concrete kinematics traction and steering commands are calculated. -Currently implemented kinematics in corresponding packages are: + +Currently implemented kinematics +-------------------------------------------------------------- * :ref:`Bicycle ` - with one steering and one drive joints; * :ref:`Tricylce ` - with one steering and two drive joints; -* :ref:`Ackermann ` - with two seering and two drive joints. +* :ref:`Ackermann ` - with two steering and two drive joints. +.. toctree:: + :hidden: + Bicycle <../../bicycle_steering_controller/doc/userdoc.rst> + Tricylce <../../tricycle_steering_controller/doc/userdoc.rst> + Ackermann <../../ackermann_steering_controller/doc/userdoc.rst> Description of controller's interfaces -------------------------------------- References (from a preceding controller) ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, -- /linear/velocity [double], in m/s -- /angular/position [double] # in [rad] -Commands -,,,,,,,,, -``front_steering == true`` +Used when controller is in chained mode (``in_chained_mode == true``). + +- ``/linear/velocity`` double, in m/s +- ``/angular/velocity`` double, in rad/s + +representing the body twist. -- /position [double] # in [rad] -- /velocity [double] # in [m/s] +Command interfaces +,,,,,,,,,,,,,,,,,,, -``front_steering == false`` +If parameter ``front_steering == true`` -- /velocity [double] # in [m/s] -- /position [double] # in [rad] +- ``/position`` double, in rad +- ``/velocity`` double, in m/s + +If parameter ``front_steering == false`` + +- ``/velocity`` double, in m/s +- ``/position`` double, in rad + +State interfaces +,,,,,,,,,,,,,,,,, -States -,,,,,,, Depending on the ``position_feedback``, different feedback types are expected * ``position_feedback == true`` --> ``TRACTION_FEEDBACK_TYPE = position`` * ``position_feedback == false`` --> ``TRACTION_FEEDBACK_TYPE = velocity`` -``front_steering == true`` +If parameter ``front_steering == true`` -- /position [double] # in [rad] -- / [double] # in [m] or [m/s] +- ``/position`` double, in rad +- ``/`` double, in m or m/s -``front_steering == false`` +If parameter ``front_steering == false`` -- / [double] # [m] or [m/s] -- /position [double] # in [rad] +- ``/`` double, in m or m/s +- ``/position`` double, in rad Subscribers ,,,,,,,,,,,, + Used when controller is not in chained mode (``in_chained_mode == false``). -- /reference [geometry_msgs/msg/TwistStamped] - If parameter ``use_stamped_vel`` is ``true``. -- /reference_unstamped [geometry_msgs/msg/Twist] - If parameter ``use_stamped_vel`` is ``false``. +- ``/reference`` [`geometry_msgs/msg/TwistStamped `_] Publishers ,,,,,,,,,,, -- /odometry [nav_msgs/msg/Odometry] -- /tf_odometry [tf2_msgs/msg/TFMessage] -- /controller_state [control_msgs/msg/SteeringControllerStatus] + +- ``/odometry`` [`nav_msgs/msg/Odometry `_] +- ``/tf_odometry`` [`tf2_msgs/msg/TFMessage `_] +- ``/controller_state`` [`control_msgs/msg/SteeringControllerStatus `_] Parameters ,,,,,,,,,,, + This controller uses the `generate_parameter_library `_ to handle its parameters. For an exemplary parameterization see the ``test`` folder of the controller's package. diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index b560e2a782..84a892d79e 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -15,21 +15,16 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ -#include #include #include -#include #include -#include #include #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -#include "std_srvs/srv/set_bool.hpp" #include "steering_controllers_library/steering_odometry.hpp" #include "steering_controllers_library/visibility_control.h" #include "steering_controllers_library_parameters.hpp" @@ -37,7 +32,6 @@ // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #include "control_msgs/msg/steering_controller_status.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -98,7 +92,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; - rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms @@ -132,7 +125,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // name constants for reference interfaces size_t nr_ref_itfs_; - // store last velocity + // last velocity commands for open loop odometry double last_linear_velocity_ = 0.0; double last_angular_velocity_ = 0.0; @@ -143,7 +136,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // callback for topic interface STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( const std::shared_ptr msg); - void reference_callback_unstamped(const std::shared_ptr msg); }; } // namespace steering_controllers_library diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 95bcef7e63..5b67797b79 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -18,19 +18,27 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ +#include #include #include -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 #include "rcpputils/rolling_mean_accumulator.hpp" +#else +#include "rcppmath/rolling_mean_accumulator.hpp" +#endif namespace steering_odometry { const unsigned int BICYCLE_CONFIG = 0; const unsigned int TRICYCLE_CONFIG = 1; const unsigned int ACKERMANN_CONFIG = 2; + +inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; } + /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) @@ -56,7 +64,7 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest wheels position * \param traction_wheel_pos traction wheel position [rad] - * \param steer_pos Front Steer position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -67,7 +75,7 @@ class SteeringOdometry * \brief Updates the odometry class with latest wheels position * \param right_traction_wheel_pos Right traction wheel velocity [rad] * \param left_traction_wheel_pos Left traction wheel velocity [rad] - * \param front_steer_pos Steer wheel position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -91,7 +99,7 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest wheels position * \param traction_wheel_vel Traction wheel velocity [rad/s] - * \param front_steer_pos Steer wheel position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -102,7 +110,7 @@ class SteeringOdometry * \brief Updates the odometry class with latest wheels position * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] - * \param front_steer_pos Steer wheel position [rad] + * \param steer_pos Steer wheel position [rad] * \param dt time difference to last call * \return true if the odometry is actually updated */ @@ -125,11 +133,11 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest velocity command - * \param linear Linear velocity [m/s] - * \param angular Angular velocity [rad/s] - * \param time Current time + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - void update_open_loop(const double linear, const double angular, const double dt); + void update_open_loop(const double v_bx, const double omega_bz, const double dt); /** * \brief Set odometry type @@ -170,22 +178,24 @@ class SteeringOdometry /** * \brief Sets the wheel parameters: radius, separation and wheelbase */ - void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0); + void set_wheel_params( + const double wheel_radius, const double wheelbase = 0.0, const double wheel_track = 0.0); /** * \brief Velocity rolling window size setter * \param velocity_rolling_window_size Velocity rolling window size */ - void set_velocity_rolling_window_size(size_t velocity_rolling_window_size); + void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size); /** * \brief Calculates inverse kinematics for the desired linear and angular velocities - * \param Vx Desired linear velocity [m/s] - * \param theta_dot Desired angular velocity [rad/s] + * \param v_bx Desired linear velocity of the robot in x_b-axis direction + * \param omega_bz Desired angular velocity of the robot around x_z-axis + * \param open_loop If false, the IK will be calculated using measured steering angle * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double Vx, const double theta_dot); + const double v_bx, const double omega_bz, const bool open_loop = true); /** * \brief Reset poses, heading, and accumulators @@ -194,41 +204,58 @@ class SteeringOdometry private: /** - * \brief Uses precomputed linear and angular velocities to compute dometry and update - * accumulators \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) - * computed by previous odometry method \param angular Angular velocity [rad] (angular - * displacement, i.e. m/s * dt) computed by previous odometry method + * \brief Uses precomputed linear and angular velocities to compute odometry + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - bool update_odometry(const double linear_velocity, const double angular, const double dt); + bool update_odometry(const double v_bx, const double omega_bz, const double dt); /** * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by - * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed - * by encoders + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - void integrate_runge_kutta_2(double linear, double angular); + void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt); /** - * \brief Integrates the velocities (linear and angular) using exact method - * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by - * encoders \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed - * by encoders + * \brief Integrates the velocities (linear and angular) + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] + * \param dt time difference to last call */ - void integrate_exact(double linear, double angular); + void integrate_fk(const double v_bx, const double omega_bz, const double dt); /** - * \brief Calculates steering angle from the desired translational and rotational velocity - * \param Vx Linear velocity [m] - * \param theta_dot Angular velocity [rad] + * \brief Calculates steering angle from the desired twist + * \param v_bx Linear velocity of the robot in x_b-axis direction + * \param omega_bz Angular velocity of the robot around x_z-axis */ - double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); + double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + + /** + * \brief Calculates linear velocity of a robot with double traction axle + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param steer_pos Steer wheel position [rad] + */ + double get_linear_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos); /** * \brief Reset linear and angular accumulators */ void reset_accumulators(); +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 + using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; +#else + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; +#endif + /// Current timestamp: rclcpp::Time timestamp_; @@ -256,8 +283,8 @@ class SteeringOdometry double traction_left_wheel_old_pos_; /// Rolling mean accumulators for the linear and angular velocities: size_t velocity_rolling_window_size_; - rcpputils::RollingMeanAccumulator linear_acc_; - rcpputils::RollingMeanAccumulator angular_acc_; + RollingMeanAccumulator linear_acc_; + RollingMeanAccumulator angular_acc_; }; } // namespace steering_odometry diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 8212f58d91..5c96c4624c 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.6.0 + 4.15.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index eb497ebb3d..836574f150 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -16,15 +16,11 @@ #include #include -#include #include #include #include -#include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "tf2/transform_datatypes.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace @@ -114,19 +110,9 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - if (params_.use_stamped_vel) - { - ref_subscriber_twist_ = get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); - } - else - { - ref_subscriber_unstamped_ = get_node()->create_subscription( - "~/reference_unstamped", subscribers_qos, - std::bind( - &SteeringControllersLibrary::reference_callback_unstamped, this, std::placeholders::_1)); - } + ref_subscriber_twist_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); @@ -241,42 +227,6 @@ void SteeringControllersLibrary::reference_callback( } } -void SteeringControllersLibrary::reference_callback_unstamped( - const std::shared_ptr msg) -{ - RCLCPP_WARN( - get_node()->get_logger(), - "Use of Twist message without stamped is deprecated and it will be removed in ROS 2 J-Turtle " - "version. Use '~/reference' topic with 'geometry_msgs::msg::TwistStamped' message type in the " - "future."); - auto twist_stamped = *(input_ref_.readFromNonRT()); - twist_stamped->header.stamp = get_node()->now(); - // if no timestamp provided use current time for command timestamp - if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u) - { - RCLCPP_WARN( - get_node()->get_logger(), - "Timestamp in header is missing, using current time as command timestamp."); - twist_stamped->header.stamp = get_node()->now(); - } - - const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp; - - if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) - { - twist_stamped->twist = *msg; - } - else - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " - "(%.4f).", - rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(), - ref_timeout_.seconds()); - } -} - controller_interface::InterfaceConfiguration SteeringControllersLibrary::command_interface_configuration() const { @@ -370,7 +320,7 @@ SteeringControllersLibrary::on_export_reference_interfaces() &reference_interfaces_[0])); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_POSITION, + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[1])); return reference_interfaces; @@ -405,26 +355,11 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - (current_ref)->header.stamp; - // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; - } - } - else - { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = 0.0; - reference_interfaces_[1] = 0.0; - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); - } + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; } return controller_interface::return_type::OK; @@ -442,17 +377,21 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - // store and set commands + // store (for open loop odometry) and set commands last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; + const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp; + const auto timeout = + age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); + auto [traction_commands, steering_commands] = - odometry_.get_commands(last_linear_velocity_, last_angular_velocity_); + odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { - command_interfaces_[i].set_value(traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); } for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { @@ -464,7 +403,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i].set_value(traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); } for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index a9f7fa75fb..711a780458 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -112,11 +112,3 @@ steering_controllers_library: position_feedback is true then ``HW_IF_POSITION`` is taken as interface type", read_only: false, } - - use_stamped_vel: { - type: bool, - default_value: false, - description: "Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if - use_stamped_vel is true then ``geometry_msgs::msg::TwistStamped`` is taken as vel msg type", - read_only: false, - } diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index e2ced036ff..dbe210ed41 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -20,7 +20,7 @@ #include "steering_controllers_library/steering_odometry.hpp" #include -#include +#include namespace steering_odometry { @@ -35,6 +35,8 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) wheelbase_(0.0), wheel_radius_(0.0), traction_wheel_old_pos_(0.0), + traction_right_wheel_old_pos_(0.0), + traction_left_wheel_old_pos_(0.0), velocity_rolling_window_size_(velocity_rolling_window_size), linear_acc_(velocity_rolling_window_size), angular_acc_(velocity_rolling_window_size) @@ -49,10 +51,10 @@ void SteeringOdometry::init(const rclcpp::Time & time) } bool SteeringOdometry::update_odometry( - const double linear_velocity, const double angular, const double dt) + const double linear_velocity, const double angular_velocity, const double dt) { /// Integrate odometry: - SteeringOdometry::integrate_exact(linear_velocity * dt, angular); + integrate_fk(linear_velocity, angular_velocity, dt); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -62,7 +64,7 @@ bool SteeringOdometry::update_odometry( /// Estimate speeds using a rolling mean to filter them out: linear_acc_.accumulate(linear_velocity); - angular_acc_.accumulate(angular / dt); + angular_acc_.accumulate(angular_velocity); linear_ = linear_acc_.getRollingMean(); angular_ = angular_acc_.getRollingMean(); @@ -73,70 +75,47 @@ bool SteeringOdometry::update_odometry( bool SteeringOdometry::update_from_position( const double traction_wheel_pos, const double steer_pos, const double dt) { - /// Get current wheel joint positions: - const double traction_wheel_cur_pos = traction_wheel_pos * wheel_radius_; - const double traction_wheel_est_pos_diff = traction_wheel_cur_pos - traction_wheel_old_pos_; + const double traction_wheel_est_pos_diff = traction_wheel_pos - traction_wheel_old_pos_; /// Update old position with current: - traction_wheel_old_pos_ = traction_wheel_cur_pos; + traction_wheel_old_pos_ = traction_wheel_pos; - /// Compute linear and angular diff: - const double linear_velocity = traction_wheel_est_pos_diff / dt; - steer_pos_ = steer_pos; - const double angular = tan(steer_pos) * linear_velocity / wheelbase_; - - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity(traction_wheel_est_pos_diff / dt, steer_pos, dt); } bool SteeringOdometry::update_from_position( const double traction_right_wheel_pos, const double traction_left_wheel_pos, const double steer_pos, const double dt) { - /// Get current wheel joint positions: - const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; - const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; - const double traction_right_wheel_est_pos_diff = - traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + traction_right_wheel_pos - traction_right_wheel_old_pos_; const double traction_left_wheel_est_pos_diff = - traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; + traction_left_wheel_pos - traction_left_wheel_old_pos_; /// Update old position with current: - traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; - traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; + traction_right_wheel_old_pos_ = traction_right_wheel_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_pos; - const double linear_velocity = - (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; - steer_pos_ = steer_pos; - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; - - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity( + traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, steer_pos, dt); } bool SteeringOdometry::update_from_position( const double traction_right_wheel_pos, const double traction_left_wheel_pos, const double right_steer_pos, const double left_steer_pos, const double dt) { - /// Get current wheel joint positions: - const double traction_right_wheel_cur_pos = traction_right_wheel_pos * wheel_radius_; - const double traction_left_wheel_cur_pos = traction_left_wheel_pos * wheel_radius_; - const double traction_right_wheel_est_pos_diff = - traction_right_wheel_cur_pos - traction_right_wheel_old_pos_; + traction_right_wheel_pos - traction_right_wheel_old_pos_; const double traction_left_wheel_est_pos_diff = - traction_left_wheel_cur_pos - traction_left_wheel_old_pos_; + traction_left_wheel_pos - traction_left_wheel_old_pos_; /// Update old position with current: - traction_right_wheel_old_pos_ = traction_right_wheel_cur_pos; - traction_left_wheel_old_pos_ = traction_left_wheel_cur_pos; + traction_right_wheel_old_pos_ = traction_right_wheel_pos; + traction_left_wheel_old_pos_ = traction_left_wheel_pos; - /// Compute linear and angular diff: - const double linear_velocity = - (traction_right_wheel_est_pos_diff + traction_left_wheel_est_pos_diff) * 0.5 / dt; - steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; - - return update_odometry(linear_velocity, angular, dt); + return update_from_velocity( + traction_right_wheel_est_pos_diff / dt, traction_left_wheel_est_pos_diff / dt, right_steer_pos, + left_steer_pos, dt); } bool SteeringOdometry::update_from_velocity( @@ -144,44 +123,71 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular = tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular_velocity, dt); +} + +double SteeringOdometry::get_linear_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos) +{ + double turning_radius = wheelbase_ / std::tan(steer_pos); + const double vel_wheel_r = right_traction_wheel_vel * wheel_radius_; + const double vel_wheel_l = left_traction_wheel_vel * wheel_radius_; + + if (std::isinf(turning_radius)) + { + return (vel_wheel_r + vel_wheel_l) * 0.5; + } - return update_odometry(linear_velocity, angular, dt); + // overdetermined, we take the average + const double vel_r = vel_wheel_r * turning_radius / (turning_radius + wheel_track_ * 0.5); + const double vel_l = vel_wheel_l * turning_radius / (turning_radius - wheel_track_ * 0.5); + return (vel_r + vel_l) * 0.5; } bool SteeringOdometry::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos, const double dt) { - double linear_velocity = - (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; steer_pos_ = steer_pos; + double linear_velocity = get_linear_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; - return update_odometry(linear_velocity, angular, dt); + return update_odometry(linear_velocity, angular_velocity, dt); } bool SteeringOdometry::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double right_steer_pos, const double left_steer_pos, const double dt) { - steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; - double linear_velocity = - (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; - const double angular = steer_pos_ * linear_velocity / wheelbase_; - - return update_odometry(linear_velocity, angular, dt); + // overdetermined, we take the average + const double right_steer_pos_est = std::atan( + wheelbase_ * std::tan(right_steer_pos) / + (wheelbase_ - wheel_track_ / 2 * std::tan(right_steer_pos))); + const double left_steer_pos_est = std::atan( + wheelbase_ * std::tan(left_steer_pos) / + (wheelbase_ + wheel_track_ / 2 * std::tan(left_steer_pos))); + steer_pos_ = (right_steer_pos_est + left_steer_pos_est) * 0.5; + + double linear_velocity = get_linear_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); + const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular_velocity, dt); } -void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt) +void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt) { /// Save last linear and angular velocity: - linear_ = linear; - angular_ = angular; + linear_ = v_bx; + angular_ = omega_bz; /// Integrate odometry: - SteeringOdometry::integrate_exact(linear * dt, angular * dt); + integrate_fk(v_bx, omega_bz, dt); } void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track) @@ -200,78 +206,94 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } -double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot) +double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { - if (theta_dot == 0 || Vx == 0) - { - return 0; - } - return std::atan(theta_dot * wheelbase_ / Vx); + // phi can be nan if both v_bx and omega_bz are zero + const auto phi = std::atan(omega_bz * wheelbase_ / v_bx); + return std::isfinite(phi) ? phi : 0.0; } std::tuple, std::vector> SteeringOdometry::get_commands( - const double Vx, const double theta_dot) + const double v_bx, const double omega_bz, const bool open_loop) { - // desired velocity and steering angle of the middle of traction and steering axis - double Ws, alpha; + // desired wheel speed and steering angle of the middle of traction and steering axis + double Ws, phi, phi_IK = steer_pos_; - if (Vx == 0 && theta_dot != 0) +#if 0 + if (v_bx == 0 && omega_bz != 0) { - alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; - Ws = abs(theta_dot) * wheelbase_ / wheel_radius_; + // TODO(anyone) this would be only possible if traction is on the steering axis + phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; } else { - alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot); - Ws = Vx / (wheel_radius_ * std::cos(steer_pos_)); + // TODO(anyone) this would be valid only if traction is on the steering axis + Ws = v_bx / (wheel_radius_ * std::cos(phi_IK)); // using the measured steering angle + } +#endif + // steering angle + phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + if (open_loop) + { + phi_IK = phi; } + // wheel speed + Ws = v_bx / wheel_radius_; if (config_type_ == BICYCLE_CONFIG) { std::vector traction_commands = {Ws}; - std::vector steering_commands = {alpha}; + std::vector steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } else if (config_type_ == TRICYCLE_CONFIG) { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + // double-traction axle + if (is_close_to_zero(phi_IK)) { + // avoid division by zero traction_commands = {Ws, Ws}; } else { - double turning_radius = wheelbase_ / std::tan(steer_pos_); - double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheelbase_ / std::tan(phi_IK); + const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } - steering_commands = {alpha}; + // simple steering + steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } else if (config_type_ == ACKERMANN_CONFIG) { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + if (is_close_to_zero(phi_IK)) { + // avoid division by zero traction_commands = {Ws, Ws}; - steering_commands = {alpha, alpha}; + // shortcut, no steering + steering_commands = {phi, phi}; } else { - double turning_radius = wheelbase_ / std::tan(steer_pos_); - double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheelbase_ / std::tan(phi_IK); + const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; - double numerator = 2 * wheelbase_ * std::sin(alpha); - double denominator_first_member = 2 * wheelbase_ * std::cos(alpha); - double denominator_second_member = wheel_track_ * std::sin(alpha); + const double numerator = 2 * wheelbase_ * std::sin(phi); + const double denominator_first_member = 2 * wheelbase_ * std::cos(phi); + const double denominator_second_member = wheel_track_ * std::sin(phi); - double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member); - double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member); + const double alpha_r = + std::atan2(numerator, denominator_first_member + denominator_second_member); + const double alpha_l = + std::atan2(numerator, denominator_first_member - denominator_second_member); steering_commands = {alpha_r, alpha_l}; } return std::make_tuple(traction_commands, steering_commands); @@ -290,42 +312,43 @@ void SteeringOdometry::reset_odometry() reset_accumulators(); } -void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) +void SteeringOdometry::integrate_runge_kutta_2( + const double v_bx, const double omega_bz, const double dt) { - const double direction = heading_ + angular * 0.5; + // Compute intermediate value of the heading + const double theta_mid = heading_ + omega_bz * 0.5 * dt; - /// Runge-Kutta 2nd order integration: - x_ += linear * cos(direction); - y_ += linear * sin(direction); - heading_ += angular; + // Use the intermediate values to update the state + x_ += v_bx * std::cos(theta_mid) * dt; + y_ += v_bx * std::sin(theta_mid) * dt; + heading_ += omega_bz * dt; } -/** - * \brief Other possible integration method provided by the class - * \param linear - * \param angular - */ -void SteeringOdometry::integrate_exact(double linear, double angular) +void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt) { - if (fabs(angular) < 1e-6) + const double delta_x_b = v_bx * dt; + const double delta_theta = omega_bz * dt; + + if (is_close_to_zero(delta_theta)) { - integrate_runge_kutta_2(linear, angular); + /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero): + integrate_runge_kutta_2(v_bx, omega_bz, dt); } else { - /// Exact integration (should solve problems when angular is zero): + /// Exact integration const double heading_old = heading_; - const double r = linear / angular; - heading_ += angular; - x_ += r * (sin(heading_) - sin(heading_old)); - y_ += -r * (cos(heading_) - cos(heading_old)); + const double R = delta_x_b / delta_theta; + heading_ += delta_theta; + x_ += R * (sin(heading_) - std::sin(heading_old)); + y_ += -R * (cos(heading_) - std::cos(heading_old)); } } void SteeringOdometry::reset_accumulators() { - linear_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); - angular_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); + linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); + angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_); } } // namespace steering_odometry diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml index d200d34961..01bfb02302 100644 --- a/steering_controllers_library/test/steering_controllers_library_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -6,7 +6,6 @@ test_steering_controllers_library: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steering_joint, front_left_steering_joint] diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 0217567a26..3378efbef8 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -17,11 +17,8 @@ #include #include #include -#include #include -#include "hardware_interface/types/hardware_interface_type_values.hpp" - class SteeringControllersLibraryTest : public SteeringControllersLibraryFixture { @@ -66,16 +63,16 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + 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]); } } @@ -136,8 +133,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -145,10 +142,13 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } - for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) - { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); - } + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); // case 2 position_feedback = true controller_->params_.position_feedback = true; @@ -177,8 +177,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -186,10 +186,13 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } - for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) - { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); - } + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); } int main(int argc, char ** argv) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 1284b72096..93ee823e0f 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -15,21 +15,19 @@ #ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ #define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "steering_controllers_library/steering_controllers_library.hpp" @@ -80,14 +78,7 @@ class TestableSteeringControllersLibrary controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -101,26 +92,24 @@ class TestableSteeringControllersLibrary * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + wait_for_command(executor, timeout); } // implementing methods which are declared virtual in the steering_controllers_library.hpp @@ -139,9 +128,6 @@ class TestableSteeringControllersLibrary } bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -235,36 +221,43 @@ class SteeringControllersLibraryFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_steering_controllers_library/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) @@ -301,7 +294,6 @@ class SteeringControllersLibraryFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; @@ -325,7 +317,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 173c76baef..e3c8db6c15 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gmock/gmock.h" +#include #include "steering_controllers_library/steering_odometry.hpp" @@ -28,7 +28,21 @@ TEST(TestSteeringOdometry, initialize) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) +// ----------------- Ackermann ----------------- + +TEST(TestSteeringOdometry, ackermann_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +TEST(TestSteeringOdometry, ackermann_odometry_openloop_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -39,7 +53,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -52,7 +66,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) EXPECT_GT(odom.get_y(), 0); // pos y, ie. left } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -64,13 +78,13 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) EXPECT_LT(odom.get_y(), 0); // neg y ie. right } -TEST(TestSteeringOdometry, ackermann_back_kin_linear) +TEST(TestSteeringOdometry, ackermann_IK_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_open_loop(1., 0., 1.); - auto cmd = odom.get_commands(1., 0.); + auto cmd = odom.get_commands(1., 0., true); auto cmd0 = std::get<0>(cmd); // vel EXPECT_EQ(cmd0[0], cmd0[1]); // linear EXPECT_GT(cmd0[0], 0); @@ -79,13 +93,13 @@ TEST(TestSteeringOdometry, ackermann_back_kin_linear) EXPECT_EQ(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_left) +TEST(TestSteeringOdometry, ackermann_IK_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_from_position(0., 0.2, 1.); // assume already turn - auto cmd = odom.get_commands(1., 0.1); + auto cmd = odom.get_commands(1., 0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) EXPECT_GT(cmd0[0], 0); @@ -94,13 +108,13 @@ TEST(TestSteeringOdometry, ackermann_back_kin_left) EXPECT_GT(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_right) +TEST(TestSteeringOdometry, ackermann_IK_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); odom.update_from_position(0., -0.2, 1.); // assume already turn - auto cmd = odom.get_commands(1., -0.1); + auto cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) EXPECT_GT(cmd0[0], 0); @@ -108,3 +122,112 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right) EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) EXPECT_LT(cmd1[0], 0); } + +// ----------------- bicycle ----------------- + +TEST(TestSteeringOdometry, bicycle_IK_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_DOUBLE_EQ(cmd1[0], 0); // no steering +} + +TEST(TestSteeringOdometry, bicycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, bicycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // left steering +} + +TEST(TestSteeringOdometry, bicycle_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +// ----------------- tricycle ----------------- + +TEST(TestSteeringOdometry, tricycle_IK_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_EQ(cmd1[0], 0); // no steering +} + +TEST(TestSteeringOdometry, tricycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // left steering +} + +TEST(TestSteeringOdometry, tricycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, tricycle_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index cf2a599b20..34520eeb83 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,59 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* rename get/set_state to get/set_lifecylce_state (`#1250 `_) +* Contributors: Manuel Muth + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Add missing includes (`#1226 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich + +4.9.0 (2024-06-05) +------------------ +* Add mobile robot kinematics 101 and improve steering library docs (`#954 `_) +* Bump version of pre-commit hooks (`#1157 `_) +* Contributors: Christoph Fröhlich, github-actions[bot] + +4.8.0 (2024-05-14) +------------------ +* Add parameter check for geometric values (`#1120 `_) +* Deprecate non-stamped twist for tricycle_controller and steering_controllers (`#1093 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.7.0 (2024-03-22) +------------------ +* added conditioning to have rolling tags compilable in older versions (`#1071 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Sai Kishor Kothakota, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Fix usage of M_PI on Windows (`#1036 `_) diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index ea1c4db3c1..6834b753a0 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -29,6 +29,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(tricycle_controller_parameters src/tricycle_controller_parameter.yaml @@ -74,10 +76,8 @@ if(BUILD_TESTING) tf2_msgs ) - 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 - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_tricycle_controller test/test_load_tricycle_controller.cpp) target_link_libraries(test_load_tricycle_controller tricycle_controller ) diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml index fba6f34bb2..a7853954d7 100644 --- a/tricycle_controller/config/tricycle_drive_controller.yaml +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -36,7 +36,6 @@ tricycle_controller: # cmd_vel input cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received - use_stamped_vel: false # Set to True if using TwistStamped. # Debug publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index 991627eca2..2ede52e1b6 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -10,12 +10,7 @@ Controller for mobile robots with a single double-actuated wheel, including trac Input for control are robot base_link twist commands which are translated to traction and steering commands for the tricycle drive base. Odometry is computed from hardware feedback and published. -Velocity commands ------------------ - -The controller works with a velocity twist from which it extracts -the x component of the linear velocity and the z component of the angular velocity. -Velocities on other components are ignored. +For an introduction to mobile robot kinematics and the nomenclature used here, see :ref:`mobile_robot_kinematics`. Other features @@ -26,6 +21,16 @@ Other features Velocity, acceleration and jerk limits Automatic stop after command timeout +ROS 2 Interfaces +------------------------ + +Subscribers +,,,,,,,,,,,, + +~/cmd_vel [geometry_msgs/msg/TwistStamped] + Velocity command for the controller. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + + Parameters -------------- diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp index 62eb51d6cc..f3252df12f 100644 --- a/tricycle_controller/include/tricycle_controller/odometry.hpp +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -21,8 +21,13 @@ #include -#include "rclcpp/time.hpp" +#include +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 #include "rcpputils/rolling_mean_accumulator.hpp" +#else +#include "rcppmath/rolling_mean_accumulator.hpp" +#endif namespace tricycle_controller { @@ -45,7 +50,12 @@ class Odometry void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); private: +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6 using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator; +#else + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; +#endif void integrateRungeKutta2(double linear, double angular); void integrateExact(double linear, double angular); diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 24aaf89de9..010a890f52 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -31,12 +31,9 @@ #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_box.h" -#include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -140,8 +137,6 @@ class TricycleController : public controller_interface::ControllerInterface bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - rclcpp::Subscription::SharedPtr - velocity_command_unstamped_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 04995732f2..4e6ab510f3 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.6.0 + 4.15.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 07be8c2aae..ec7ca7bd5e 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -86,7 +86,7 @@ InterfaceConfiguration TricycleController::state_interface_configuration() const controller_interface::return_type TricycleController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) { @@ -239,7 +239,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & 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 { @@ -291,51 +290,27 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } // initialize command subscriber - if (params_.use_stamped_vel) - { - velocity_command_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) - { - RCLCPP_WARN_ONCE( - get_node()->get_logger(), - "Received TwistStamped with zero timestamp, setting it to current " - "time, this message will only be shown once"); - msg->header.stamp = get_node()->get_clock()->now(); - } - received_velocity_msg_ptr_.set(std::move(msg)); - }); - } - else - { - velocity_command_unstamped_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - - // Write fake header in the stored stamped command - std::shared_ptr twist_stamped; - received_velocity_msg_ptr_.get(twist_stamped); - twist_stamped->twist = *msg; - twist_stamped->header.stamp = get_node()->get_clock()->now(); - }); - } - - // initialize odometry publisher and messasge + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.set(std::move(msg)); + }); + + // initialize odometry publisher and message odometry_publisher_ = get_node()->create_publisher( DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_odometry_publisher_ = @@ -457,7 +432,6 @@ bool TricycleController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - velocity_command_unstamped_subscriber_.reset(); received_velocity_msg_ptr_.set(nullptr); is_halted = false; diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml index 68fc2202c2..cf661eddc1 100644 --- a/tricycle_controller/src/tricycle_controller_parameter.yaml +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -19,11 +19,17 @@ tricycle_controller: 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.", + validation: { + gt<>: [0.0] + } } wheel_radius: { type: double, default_value: 0.0, description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + validation: { + gt<>: [0.0] + } } odom_frame_id: { type: string, @@ -78,11 +84,6 @@ tricycle_controller: 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 " diff --git a/tricycle_controller/test/config/test_tricycle_controller.yaml b/tricycle_controller/test/config/test_tricycle_controller.yaml index efecfe968f..4cef3ee131 100644 --- a/tricycle_controller/test/config/test_tricycle_controller.yaml +++ b/tricycle_controller/test/config/test_tricycle_controller.yaml @@ -5,7 +5,6 @@ test_tricycle_controller: steering_joint_name: steering_joint wheel_radius: 0.125 wheelbase: 1.252 - use_stamped_vel: false enable_odom_tf: false use_sim_time: false pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index bd54459780..7f7f0e6fed 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -32,13 +32,14 @@ TEST(TestLoadTricycleController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/config/test_tricycle_controller.yaml"; - ASSERT_NE( - cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), - nullptr); + cm.set_parameter({"test_tricycle_controller.params_file", test_file_path}); + cm.set_parameter({"test_tricycle_controller.type", "tricycle_controller/TricycleController"}); + + ASSERT_NE(cm.load_controller("test_tricycle_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 5280aaf244..9d43c2590d 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -18,7 +18,6 @@ #include -#include #include #include #include @@ -29,7 +28,8 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "tricycle_controller/tricycle_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; @@ -62,22 +62,17 @@ class TestableTricycleController : public tricycle_controller::TricycleControlle * @brief wait_for_twist block until a new twist is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function - * - * @return true if new twist msg was received, false if timeout */ - bool wait_for_twist( + void wait_for_twist( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) { - rclcpp::WaitSet wait_set; - wait_set.add_subscription(velocity_command_subscriber_); - - if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); - return true; + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return false; } }; @@ -164,6 +159,9 @@ class TestTricycleController : public ::testing::Test 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))); + // default parameters + parameter_overrides.push_back(rclcpp::Parameter("wheelbase", rclcpp::ParameterValue(1.))); + parameter_overrides.push_back(rclcpp::Parameter("wheel_radius", rclcpp::ParameterValue(0.1))); parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); node_options.parameter_overrides(parameter_overrides); @@ -323,7 +321,7 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) const double angular = 0.0; publish(linear, angular); // wait for msg is be published to the system - ASSERT_TRUE(controller_->wait_for_twist(executor)); + controller_->wait_for_twist(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 718df4e014..4e1f9abd1e 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,59 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Add missing includes (`#1226 `_) +* Change the subscription timeout in the tests to 5ms (`#1219 `_) +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Christoph Fröhlich, Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Fix steering controllers library kinematics (`#1150 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- +* [Steering controllers library] Reference interfaces are body twist (`#1168 `_) +* Fix steering controllers library code documentation and naming (`#1149 `_) +* Remove unstamped twist subscribers + parameters (`#1151 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Quique Llorente + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ +* Add parameter check for geometric values (`#1120 `_) +* add missing compiler definitions of RCPPUTILS_VERSION (`#1089 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + +4.7.0 (2024-03-22) +------------------ +* Fix pid_controller build on ROS 2 Rolling on Ubuntu 24.04 (`#1084 `_) +* Fix usage of visibility macros (`#1039 `_) +* Contributors: Chris Lalancette, Silvio Traversaro + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 21604df0c4..24b4cd1a22 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -24,6 +24,8 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) generate_parameter_library(tricycle_steering_controller_parameters src/tricycle_steering_controller.yaml @@ -55,10 +57,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_tricycle_steering_controller - test/test_load_tricycle_steering_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_tricycle_steering_controller test/test_load_tricycle_steering_controller.cpp) ament_target_dependencies(test_load_tricycle_steering_controller controller_manager hardware_interface diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index bed6e451fa..27aac82dd1 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.6.0 + 4.15.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar @@ -18,6 +18,7 @@ generate_parameter_library + backward_ros control_msgs controller_interface hardware_interface diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index f89d78a52c..03be6b88f0 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -60,24 +60,28 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period } else { - const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); - const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double steer_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + const double traction_right_wheel_value = + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + const double traction_left_wheel_value = + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); if ( - !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && - !std::isnan(steer_position)) + std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && + std::isfinite(steering_position)) { if (params_.position_feedback) { // Estimate linear and angular velocity using joint information odometry_.update_from_position( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); } else { // Estimate linear and angular velocity using joint information odometry_.update_from_velocity( - rear_right_wheel_value, rear_left_wheel_value, steer_position, period.seconds()); + traction_right_wheel_value, traction_left_wheel_value, steering_position, + period.seconds()); } } } diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml index 1015865fd9..6e5ae2b477 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.yaml +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -5,6 +5,9 @@ tricycle_steering_controller: default_value: 0.0, description: "Wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } wheelbase: @@ -13,6 +16,9 @@ tricycle_steering_controller: default_value: 0.0, description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + validation: { + gt<>: [0.0] + } } front_wheels_radius: @@ -21,6 +27,9 @@ tricycle_steering_controller: default_value: 0.0, description: "Front wheels radius.", read_only: false, + validation: { + gt<>: [0.0] + } } rear_wheels_radius: @@ -29,4 +38,7 @@ tricycle_steering_controller: default_value: 0.0, description: "Rear wheels radius.", read_only: false, + validation: { + gt<>: [0.0] + } } diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index 0d04afdf38..d3579c902f 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -28,15 +28,16 @@ TEST(TestLoadTricycleSteeringController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/tricycle_steering_controller_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_tricycle_steering_controller", - "tricycle_steering_controller/TricycleSteeringController"), - nullptr); + cm.set_parameter({"test_tricycle_steering_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_tricycle_steering_controller.type", + "tricycle_steering_controller/TricycleSteeringController"}); + + ASSERT_NE(cm.load_controller("test_tricycle_steering_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index c555de53de..8e29314f8e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -14,10 +14,8 @@ #include "test_tricycle_steering_controller.hpp" -#include #include #include -#include #include class TricycleSteeringControllerTest @@ -83,9 +81,9 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); + 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]); } } @@ -201,6 +199,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -240,12 +239,13 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu EXPECT_EQ(msg.steering_angle_command[0], 2.2); publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); + controller_->wait_for_commands(executor); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -258,6 +258,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu subscribe_and_get_messages(msg); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 6a516691b8..3b7a053937 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -15,21 +15,19 @@ #ifndef TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ #define TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ +#include + #include -#include #include #include -#include #include #include -#include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/parameter_value.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "tricycle_steering_controller/tricycle_steering_controller.hpp" @@ -75,14 +73,7 @@ class TestableTricycleSteeringController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - auto ret = - tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); - // Only if on_configure is successful create subscription - if (ret == CallbackReturn::SUCCESS) - { - ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); - } - return ret; + return tricycle_steering_controller::TricycleSteeringController::on_configure(previous_state); } controller_interface::CallbackReturn on_activate( @@ -96,30 +87,25 @@ class TestableTricycleSteeringController * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. - * - * @return true if new ControllerReferenceMsg msg was received, false if timeout. */ - bool wait_for_command( - rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + void wait_for_command( + rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; - if (success) + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) { executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); } - return success; } - bool wait_for_commands( + void wait_for_commands( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + return wait_for_command(executor, timeout); } - -private: - rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -201,34 +187,43 @@ class TricycleSteeringControllerFixture : public ::testing::Test void subscribe_and_get_messages(ControllerStateMsg & msg) { // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; rclcpp::Node test_subscription_node("test_subscription_node"); - auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( "/test_tricycle_steering_controller/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); // call update to publish the test value ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // call update to publish the test value // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message - wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // check if message has been received - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + if (received_msg.get()) { break; } } ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); // take message from subscription - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(msg, msg_info)); + msg = *received_msg; } void publish_commands(const double linear = 0.1, const double angular = 0.2) @@ -265,7 +260,6 @@ class TricycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = {"steering_axis_joint"}; std::vector joint_names_ = { @@ -286,7 +280,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; 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 6f2913aeb8..566169e34e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -14,10 +14,8 @@ #include "test_tricycle_steering_controller.hpp" -#include #include #include -#include #include class TricycleSteeringControllerTest @@ -86,9 +84,9 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); + 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/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml index 6bfb87a892..cc640e1503 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_tricycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [steering_axis_joint] diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml index ea8b88002e..7cb2e4906f 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_tricycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] front_wheels_names: [pid_controller/steering_axis_joint] rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e6689b23a8..a04bf1c4ae 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,42 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- +* Unused header cleanup (`#1199 `_) +* Fix WaitSet issue in tests (`#1206 `_) +* Fix parallel gripper controller CI (`#1202 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + +4.11.0 (2024-07-09) +------------------- +* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) +* Contributors: Sai Kishor Kothakota + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + 4.6.0 (2024-02-12) ------------------ * Add test_depend on `hardware_interface_testing` (`#1018 `_) diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp index b5c36f433a..e443ba76cc 100644 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp @@ -15,10 +15,7 @@ #ifndef VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ #define VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ -#include - #include "forward_command_controller/forward_command_controller.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "velocity_controllers/visibility_control.h" namespace velocity_controllers diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index ff403e178f..ea295db0ed 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.6.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp index eb55e052dc..97f6713d72 100644 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ b/velocity_controllers/src/joint_group_velocity_controller.cpp @@ -16,7 +16,6 @@ #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/logging.hpp" #include "rclcpp/parameter.hpp" #include "velocity_controllers/joint_group_velocity_controller.hpp" diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index a99ffaeebf..d781b6ca5c 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -12,35 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include #include #include #include #include #include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_joint_group_velocity_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void JointGroupVelocityControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void JointGroupVelocityControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -63,6 +47,7 @@ void JointGroupVelocityControllerTest::SetUpController() command_ifs.emplace_back(joint_2_cmd_); command_ifs.emplace_back(joint_3_cmd_); controller_->assign_interfaces(std::move(command_ifs), {}); + executor.add_node(controller_->get_node()->get_node_base_interface()); } TEST_F(JointGroupVelocityControllerTest, JointsParameterNotSet) @@ -204,10 +189,13 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) command_pub->publish(command_msg); // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{10}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } // update successful ASSERT_EQ( diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index e94f7f082d..0ebe68833c 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -15,14 +15,15 @@ #ifndef TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ #define TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ +#include + #include #include #include -#include "gmock/gmock.h" - #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" #include "velocity_controllers/joint_group_velocity_controller.hpp" using hardware_interface::CommandInterface; @@ -58,6 +59,7 @@ class JointGroupVelocityControllerTest : public ::testing::Test CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]}; CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]}; CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]}; + rclcpp::executors::SingleThreadedExecutor executor; }; #endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp index e426349f96..a7dc72f337 100644 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp @@ -30,9 +30,7 @@ TEST(TestLoadJointGroupVelocityController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( cm.load_controller(