diff --git a/.clang-format b/.clang-format index ab8d077577..9e92cf780a 100644 --- a/.clang-format +++ b/.clang-format @@ -10,5 +10,5 @@ ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 DerivePointerAlignment: false PointerAlignment: Middle -ReflowComments: false +ReflowComments: true IncludeBlocks: Preserve diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 6bbf2e957a..95181eceb0 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -28,7 +28,7 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa - [ ] 🙋 **Claim this issue**: Comment below. If someone else has claimed it, ask if they've opened a pull request already and if they're stuck -- maybe you can help them solve a problem or move it along! -- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/foxy/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling). +- [ ] 🗄️ **Create a local workspace** for making your changes and testing [following these instructions](https://docs.ros.org/en/rolling/Tutorials/Workspace/Creating-A-Workspace.html), for Step3 use "Download Source Code" section with [these instructions](https://ros-controls.github.io/control.ros.org/getting_started.html#compiling). - [ ] 🍴 **Fork the repository** using the handy button at the top of the repository page and **clone** it into `~/ws_ros2_control/src/ros-controls/ros2_controllers`, [here is a guide that you can follow](https://guides.github.com/activities/forking/) (You will have to remove or empty the existing `ros2_controllers` folder before cloning your own fork) @@ -54,7 +54,7 @@ Nothing. This issue is meant to welcome you to Open Source :) We are happy to wa Don’t hesitate to ask questions or to get help if you feel like you are getting stuck. For example leave a comment below! Furthermore, you find helpful resources here: * [ROS2 Control Contribution Guide](https://ros-controls.github.io/control.ros.org/contributing.html) -* [ROS2 Tutorials](https://docs.ros.org/en/foxy/Tutorials.html) +* [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) * [ROS Answers](https://answers.ros.org/questions/) **Good luck with your first issue!** diff --git a/.github/mergify.yml b/.github/mergify.yml index 87490702f6..49d2acedfa 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -1,12 +1,12 @@ pull_request_rules: - - name: Backport to foxy at reviewers discretion + - name: Backport to humble at reviewers discretion conditions: - base=master - - "label=backport-foxy" + - "label=backport-humble" actions: backport: branches: - - foxy + - humble - name: Ask to resolve conflict conditions: diff --git a/.github/workflows/README.md b/.github/workflows/README.md index 5789859350..7727495a97 100644 --- a/.github/workflows/README.md +++ b/.github/workflows/README.md @@ -4,7 +4,6 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`rolling`](https://github.com/ros-controls/ros2_controllers/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-testing.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-testing.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-source-build.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) **Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-testing.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-testing.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-source-build.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) -**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-testing.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-testing.yml?branch=foxy) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) ### Explanation of different build types diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 6e6e96f032..67d4c43375 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,11 +16,11 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.3.0 + - uses: ros-tooling/action-ros-ci@0.3.2 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} @@ -37,7 +37,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.3 + - uses: codecov/codecov-action@v3.1.4 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index f0993bf717..8f5be014c5 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.6.0 + - uses: actions/setup-python@v4.6.1 with: python-version: '3.10' - name: Install system hooks diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index fd3e6b634b..3149bba54d 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/foxy-abi-compatibility.yml b/.github/workflows/foxy-abi-compatibility.yml deleted file mode 100644 index 7ce17effd0..0000000000 --- a/.github/workflows/foxy-abi-compatibility.yml +++ /dev/null @@ -1,20 +0,0 @@ -name: Foxy - ABI Compatibility Check -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: foxy - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.github/workflows/foxy-binary-build-main.yml b/.github/workflows/foxy-binary-build-main.yml deleted file mode 100644 index b306d7e44d..0000000000 --- a/.github/workflows/foxy-binary-build-main.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Foxy Binary Build - main -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: foxy - ros_repo: main - upstream_workspace: ros2_controllers-not-released.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-binary-build-testing.yml b/.github/workflows/foxy-binary-build-testing.yml deleted file mode 100644 index 260751abea..0000000000 --- a/.github/workflows/foxy-binary-build-testing.yml +++ /dev/null @@ -1,26 +0,0 @@ -name: Foxy Binary Build - testing -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: foxy - ros_repo: testing - upstream_workspace: ros2_controllers-not-released.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build-main.yml b/.github/workflows/foxy-semi-binary-build-main.yml deleted file mode 100644 index 75c3295124..0000000000 --- a/.github/workflows/foxy-semi-binary-build-main.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Foxy Semi-Binary Build - main -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: foxy - ros_repo: main - upstream_workspace: ros2_controllers.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/foxy-semi-binary-build-testing.yml b/.github/workflows/foxy-semi-binary-build-testing.yml deleted file mode 100644 index f6d663cc2c..0000000000 --- a/.github/workflows/foxy-semi-binary-build-testing.yml +++ /dev/null @@ -1,25 +0,0 @@ -name: Foxy Semi-Binary Build - testing -# description: 'Build & test that compiles the main dependencies from source.' - -on: - workflow_dispatch: - branches: - - foxy - pull_request: - branches: - - foxy - push: - branches: - - foxy - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ./.github/workflows/reusable-industrial-ci-with-cache.yml - with: - ros_distro: foxy - ros_repo: testing - upstream_workspace: ros2_controllers.foxy.repos - ref_for_scheduled_build: foxy diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index acbb42e16b..5e7326e510 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -9,7 +9,6 @@ on: default: 'rolling' type: choice options: - - foxy - humble - rolling branch: @@ -18,7 +17,6 @@ on: default: 'master' type: choice options: - - foxy - humble - master diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 31bc0d4475..daa378297e 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -14,7 +14,7 @@ on: required: true type: string ros2_repo_branch: - description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble, foxy.' + description: 'Branch in the ros2/ros2 repozitory from which ".repos" should be used. Possible values: master (Rolling), humble.' default: 'master' required: false type: string @@ -26,15 +26,16 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.0 + - uses: ros-tooling/action-ros-ci@0.3.2 with: target-ros2-distro: ${{ inputs.ros_distro }} + ref: ${{ inputs.ref }} # build all packages listed in the meta package package-name: diff_drive_controller diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 94f3d9bde6..772809825f 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -8,6 +8,6 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: uesteibar/reviewer-lottery@v2 + - uses: uesteibar/reviewer-lottery@v3 with: repo-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b927aeac78..5a7ac74d9b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -126,7 +126,7 @@ repos: exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks - rev: v1.9.0 + rev: v1.10.0 hooks: - id: rst-backticks exclude: CHANGELOG\.rst$ diff --git a/README.md b/README.md index d7986d34c6..8c8539a02b 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,6 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`rolling`](https://github.com/ros-controls/ros2_controllers/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#rolling) **Humble** | [`humble`](https://github.com/ros-controls/ros2_controllers/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_controllers/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#humble) -**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_controllers/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_controllers/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | | [ros2_controllers](https://index.ros.org/p/ros2_controllers/#foxy) ### Explanation of different build types diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst new file mode 100644 index 0000000000..6d5626ccd0 --- /dev/null +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -0,0 +1,141 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ackermann_steering_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Fix sphinx for steering odometry library/controllers (`#626 `_) +* Steering odometry library and controllers (`#484 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković + +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/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..6ad0e9485f --- /dev/null +++ b/ackermann_steering_controller/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.16) +project(ackermann_steering_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(ackermann_steering_controller_parameters + src/ackermann_steering_controller.yaml +) + +add_library( + ackermann_steering_controller + SHARED + src/ackermann_steering_controller.cpp +) +target_compile_features(ackermann_steering_controller PUBLIC cxx_std_17) +target_include_directories(ackermann_steering_controller PUBLIC + $ + $) +target_link_libraries(ackermann_steering_controller PUBLIC + ackermann_steering_controller_parameters) +ament_target_dependencies(ackermann_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface ackermann_steering_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + + add_rostest_with_parameters_gmock(test_load_ackermann_steering_controller + test/test_load_ackermann_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml + ) + ament_target_dependencies(test_load_ackermann_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_ackermann_steering_controller + test/test_ackermann_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml + ) + target_include_directories(test_ackermann_steering_controller PRIVATE include) + target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml) + target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller) + ament_target_dependencies( + test_ackermann_steering_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/ackermann_steering_controller +) + +install( + TARGETS ackermann_steering_controller ackermann_steering_controller_parameters + EXPORT export_ackermann_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_ackermann_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/ackermann_steering_controller/ackermann_steering_controller.xml b/ackermann_steering_controller/ackermann_steering_controller.xml new file mode 100644 index 0000000000..2ac2150dd1 --- /dev/null +++ b/ackermann_steering_controller/ackermann_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Ackermann (car-like) kinematics with two traction and two steering joints. + + + diff --git a/ackermann_steering_controller/doc/userdoc.rst b/ackermann_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..c4ae35e5b0 --- /dev/null +++ b/ackermann_steering_controller/doc/userdoc.rst @@ -0,0 +1,22 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/ackermann_steering_controller/doc/userdoc.rst + +.. _ackermann_steering_controller_userdoc: + +ackermann_steering_controller +============================= + +This controller implements the kinematics with two axes and four wheels, where the wheels on one axis are fixed (traction/drive) wheels, and the wheels on the other axis are steerable. + +The controller expects to have two commanding joints for traction, one for each fixed wheel and two commanding joints for steering, one for each wheel. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +For an exemplary parameterization see the ``test`` folder of the controller's package. + +.. generate_parameter_library_details:: ../src/ackermann_steering_controller.yaml diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp new file mode 100644 index 0000000000..0cb6bcd016 --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp @@ -0,0 +1,65 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ +#define ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ + +#include + +#include "ackermann_steering_controller/visibility_control.h" +#include "ackermann_steering_controller_parameters.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +namespace ackermann_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; +static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2; +static constexpr size_t CMD_STEER_LEFT_WHEEL = 3; + +static constexpr size_t NR_STATE_ITFS = 4; +static constexpr size_t NR_CMD_ITFS = 4; +static constexpr size_t NR_REF_ITFS = 2; + +class AckermannSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + AckermannSteeringController(); + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC void + initialize_implementation_parameter_listener() override; + +protected: + std::shared_ptr ackermann_param_listener_; + ackermann_steering_controller::Params ackermann_params_; +}; +} // namespace ackermann_steering_controller + +#endif // ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h new file mode 100644 index 0000000000..177f0bf87c --- /dev/null +++ b/ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml new file mode 100644 index 0000000000..132cac06c0 --- /dev/null +++ b/ackermann_steering_controller/package.xml @@ -0,0 +1,36 @@ + + + + ackermann_steering_controller + 3.11.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 + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp new file mode 100644 index 0000000000..c3a7539c5a --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -0,0 +1,98 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ackermann_steering_controller/ackermann_steering_controller.hpp" + +namespace ackermann_steering_controller +{ +AckermannSteeringController::AckermannSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} + +void AckermannSteeringController::initialize_implementation_parameter_listener() +{ + ackermann_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() +{ + ackermann_params_ = ackermann_param_listener_->get_params(); + + const double front_wheels_radius = ackermann_params_.front_wheels_radius; + const double rear_wheels_radius = ackermann_params_.rear_wheels_radius; + const double front_wheel_track = ackermann_params_.front_wheel_track; + const double rear_wheel_track = ackermann_params_.rear_wheel_track; + const double wheelbase = ackermann_params_.wheelbase; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track); + } + else + { + odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); + } + + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + + RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double 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(); + 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)) + { + 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()); + } + 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()); + } + } + } + return true; +} +} // namespace ackermann_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ackermann_steering_controller::AckermannSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml new file mode 100644 index 0000000000..3726146919 --- /dev/null +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -0,0 +1,40 @@ +ackermann_steering_controller: + front_wheel_track: + { + type: double, + default_value: 0.0, + description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + rear_wheel_track: + { + type: double, + default_value: 0.0, + description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + front_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Front wheels radius.", + read_only: false, + } + + rear_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Rear wheels radius.", + read_only: false, + } diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml new file mode 100644 index 0000000000..6b64f901c3 --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -0,0 +1,17 @@ +test_ackermann_steering_controller: + ros__parameters: + + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [front_right_steering_joint, front_left_steering_joint] + + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..ecb1b071e3 --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -0,0 +1,17 @@ +test_ackermann_steering_controller: + ros__parameters: + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + front_wheels_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint] + rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_state_names: [front_right_steering_joint, front_left_steering_joint] + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp new file mode 100644 index 0000000000..480e90e166 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -0,0 +1,292 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); +} + +TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_LEFT_WHEEL], + front_wheels_names_[1] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_LEFT_WHEEL], + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + + // check ref itfsTIME + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +TEST_F(AckermannSteeringControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(AckermannSteeringControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AckermannSteeringControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(AckermannSteeringControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(AckermannSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR( + + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); + EXPECT_EQ(msg.steering_angle_command[1], 4.4); + + publish_commands(); + ASSERT_TRUE(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_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); + + EXPECT_NEAR( + msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + msg.linear_velocity_command[CMD_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[1], 1.4179821977774734, COMMON_THRESHOLD); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp new file mode 100644 index 0000000000..80258258c2 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -0,0 +1,319 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ +#define TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "ackermann_steering_controller/ackermann_steering_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using ackermann_steering_controller::STATE_STEER_LEFT_WHEEL; +using ackermann_steering_controller::STATE_STEER_RIGHT_WHEEL; +using ackermann_steering_controller::STATE_TRACTION_LEFT_WHEEL; +using ackermann_steering_controller::STATE_TRACTION_RIGHT_WHEEL; + +// name constants for command interfaces +using ackermann_steering_controller::CMD_STEER_LEFT_WHEEL; +using ackermann_steering_controller::CMD_STEER_RIGHT_WHEEL; +using ackermann_steering_controller::CMD_TRACTION_LEFT_WHEEL; +using ackermann_steering_controller::CMD_TRACTION_RIGHT_WHEEL; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; +} // namespace + +// subclassing and friending so we can access member variables +class TestableAckermannSteeringController +: public ackermann_steering_controller::AckermannSteeringController +{ + FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(AckermannSteeringControllerTest, activate_success); + FRIEND_TEST(AckermannSteeringControllerTest, update_success); + FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); + FRIEND_TEST(AckermannSteeringControllerTest, reactivate_success); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic); + FRIEND_TEST(AckermannSteeringControllerTest, test_update_logic_chained); + FRIEND_TEST(AckermannSteeringControllerTest, publish_status_success); + FRIEND_TEST(AckermannSteeringControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + 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; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return ackermann_steering_controller::AckermannSteeringController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerReferenceMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool 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); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class AckermannSteeringControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_ackermann_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_ackermann_steering_controller/controller_state", 10, subs_callback); + + // 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)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + 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 + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = { + "front_right_steering_joint", "front_left_steering_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; + + double wheelbase_ = 3.24644; + double front_wheel_track_ = 2.12321; + double rear_wheel_track_ = 1.76868; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_ACKERMANN_STEERING_CONTROLLER_HPP_ diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..2d951588c5 --- /dev/null +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -0,0 +1,107 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_ackermann_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class AckermannSteeringControllerTest +: public AckermannSteeringControllerFixture +{ +}; + +TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_); + ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); +} + +TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_LEFT_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_LEFT_WHEEL], + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp new file mode 100644 index 0000000000..0a8cd7b80c --- /dev/null +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadAckermannSteeringController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_ackermann_steering_controller", + "ackermann_steering_controller/AckermannSteeringController"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 63ca4f4c0c..56feda699a 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Fix cpplint (`#681 `_) +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: Christoph Fröhlich, gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Sai Kishor Kothakota + +3.9.0 (2023-05-28) +------------------ +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-05-14) +------------------ + 3.7.0 (2023-05-02) ------------------ diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 9ece5e7fb0..a8a8832fce 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(admittance_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/admittance_controller/doc/userdoc.rst b/admittance_controller/doc/userdoc.rst index ed2da84ad5..152ed890dc 100644 --- a/admittance_controller/doc/userdoc.rst +++ b/admittance_controller/doc/userdoc.rst @@ -1,4 +1,6 @@ -.. _addmittance_controller_userdoc: +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/admittance_controller/doc/userdoc.rst + +.. _admittance_controller_userdoc: Admittance Controller ====================== @@ -16,8 +18,8 @@ Parameters ^^^^^^^^^^^ The admittance controller's uses the `generate_parameter_library `_ to handle its parameters. -The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. -An example parameter file can be found in the `test folder of the controller `_ +The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +An example parameter file can be found in the `test folder of the controller `_ Topics @@ -42,14 +44,14 @@ The controller has ``position`` and ``velocity`` reference interfaces exported i States ^^^^^^^ The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. If some interface is not provided, the last commanded interface will be used for calculation. -For handling TCP wrenches `*Force Torque Sensor* semantic component (from package *controller_interface*) `_ is used. +For handling TCP wrenches `*Force Torque Sensor* semantic component (from package *controller_interface*) `_ is used. The interfaces have prefix ``ft_sensor.name``, building the interfaces: ``/[force.x|force.y|force.z|torque.x|torque.y|torque.z]``. Commands ^^^^^^^^^ The command interfaces are defined with ``joints`` and ``command_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. +Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index e4aef2cf3d..6ff6cdae7a 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -166,20 +166,22 @@ class AdmittanceController : public controller_interface::ChainableControllerInt geometry_msgs::msg::Wrench ft_values_; /** - * @brief Read values from hardware interfaces and set corresponding fields of state_current and ft_values - */ + * @brief Read values from hardware interfaces and set corresponding fields of state_current and + * ft_values + */ void read_state_from_hardware( trajectory_msgs::msg::JointTrajectoryPoint & state_current, geometry_msgs::msg::Wrench & ft_values); /** - * @brief Set fields of state_reference with values from controllers exported position and velocity references - */ + * @brief Set fields of state_reference with values from controllers exported position and + * velocity references + */ void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state); /** -* @brief Write values from state_command to claimed hardware interfaces -*/ + * @brief Write values from state_command to claimed hardware interfaces + */ void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_command); }; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 0f0aabb063..36c027491c 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -43,13 +43,17 @@ struct AdmittanceTransforms { // transformation from force torque sensor frame to base link frame at reference joint angles Eigen::Isometry3d ref_base_ft_; - // transformation from force torque sensor frame to base link frame at reference + admittance offset joint angles + // transformation from force torque sensor frame to base link frame at reference + admittance + // offset joint angles Eigen::Isometry3d base_ft_; - // transformation from control frame to base link frame at reference + admittance offset joint angles + // transformation from control frame to base link frame at reference + admittance offset joint + // angles Eigen::Isometry3d base_control_; - // transformation from end effector frame to base link frame at reference + admittance offset joint angles + // transformation from end effector frame to base link frame at reference + admittance offset + // joint angles Eigen::Isometry3d base_tip_; - // transformation from center of gravity frame to base link frame at reference + admittance offset joint angles + // transformation from center of gravity frame to base link frame at reference + admittance offset + // joint angles Eigen::Isometry3d base_cog_; // transformation from world frame to base link frame Eigen::Isometry3d world_base_; @@ -111,20 +115,20 @@ class AdmittanceRule controller_interface::return_type reset(const size_t num_joints); /** - * Calculate all transforms needed for admittance control using the loader kinematics plugin. If the transform does - * not exist in the kinematics model, then TF will be used for lookup. The return value is true if all transformation - * are calculated without an error - * \param[in] current_joint_state current joint state of the robot - * \param[in] reference_joint_state input joint state reference - * \param[out] success true if no calls to the kinematics interface fail + * Calculate all transforms needed for admittance control using the loader kinematics plugin. If + * the transform does not exist in the kinematics model, then TF will be used for lookup. The + * return value is true if all transformation are calculated without an error \param[in] + * current_joint_state current joint state of the robot \param[in] reference_joint_state input + * joint state reference \param[out] success true if no calls to the kinematics interface fail */ bool get_all_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state); /** - * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent Eigen field - * members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, damping_) are also updated + * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent + * Eigen field members (end_effector_weight_, cog_pos_, mass_, mass_inv_ stiffness, selected_axes, + * damping_) are also updated */ void apply_parameters_update(); @@ -136,7 +140,8 @@ class AdmittanceRule * \param[in] measured_wrench most recent measured wrench from force torque sensor * \param[in] reference_joint_state input joint state reference * \param[in] period time in seconds since last controller update - * \param[out] desired_joint_state joint state reference after the admittance offset is applied to the input reference + * \param[out] desired_joint_state joint state reference after the admittance offset is applied to + * the input reference */ controller_interface::return_type update( const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, @@ -148,7 +153,8 @@ class AdmittanceRule /** * Set fields of `state_message` from current admittance controller state. * - * \param[out] state_message message containing target position/vel/accel, wrench, and actual robot state, among other things + * \param[out] state_message message containing target position/vel/accel, wrench, and actual + * robot state, among other things */ const control_msgs::msg::AdmittanceControllerState & get_controller_state(); @@ -159,22 +165,21 @@ class AdmittanceRule protected: /** - * Calculates the admittance rule from given the robot's current joint angles. The admittance controller state input - * is updated with the new calculated values. A boolean value is returned indicating if any of the kinematics plugin - * calls failed. - * \param[in] admittance_state contains all the information needed to calculate the admittance offset - * \param[in] dt controller period + * Calculates the admittance rule from given the robot's current joint angles. The admittance + * controller state input is updated with the new calculated values. A boolean value is returned + * indicating if any of the kinematics plugin calls failed. \param[in] admittance_state contains + * all the information needed to calculate the admittance offset \param[in] dt controller period * \param[out] success true if no calls to the kinematics interface fail */ bool calculate_admittance_rule(AdmittanceState & admittance_state, double dt); /** - * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement `measured_wrench`, - * the sensor to base frame rotation `sensor_world_rot`, and the center of gravity frame to base frame rotation `cog_world_rot`. - * The `wrench_world_` estimate includes gravity compensation - * \param[in] measured_wrench most recent measured wrench from force torque sensor - * \param[in] sensor_world_rot rotation matrix from world frame to sensor frame - * \param[in] cog_world_rot rotation matrix from world frame to center of gravity frame + * Updates internal estimate of wrench in world frame `wrench_world_` given the new measurement + * `measured_wrench`, the sensor to base frame rotation `sensor_world_rot`, and the center of + * gravity frame to base frame rotation `cog_world_rot`. The `wrench_world_` estimate includes + * gravity compensation \param[in] measured_wrench most recent measured wrench from force torque + * sensor \param[in] sensor_world_rot rotation matrix from world frame to sensor frame \param[in] + * cog_world_rot rotation matrix from world frame to center of gravity frame */ void process_wrench_measurements( const geometry_msgs::msg::Wrench & measured_wrench, diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 022e8e1353..ba6d549877 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 3.7.0 + 3.11.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 7dca2e2010..d6c95d2e1c 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -27,7 +27,6 @@ #include "gmock/gmock.h" -#include "6d_robot_description.hpp" #include "admittance_controller/admittance_controller.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -38,6 +37,7 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "semantic_components/force_torque_sensor.hpp" +#include "test_asset_6d_robot_description.hpp" #include "tf2_ros/transform_broadcaster.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" diff --git a/admittance_controller/test/6d_robot_description.hpp b/admittance_controller/test/test_asset_6d_robot_description.hpp similarity index 98% rename from admittance_controller/test/6d_robot_description.hpp rename to admittance_controller/test/test_asset_6d_robot_description.hpp index f67b5bd054..4d38df7c30 100644 --- a/admittance_controller/test/6d_robot_description.hpp +++ b/admittance_controller/test/test_asset_6d_robot_description.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ -#define ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#ifndef TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ +#define TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ #include @@ -310,4 +310,4 @@ const auto valid_6d_robot_srdf = } // namespace ros2_control_test_assets -#endif // ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#endif // TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst new file mode 100644 index 0000000000..1b7a761aeb --- /dev/null +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -0,0 +1,141 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package bicycle_steering_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Fix sphinx for steering odometry library/controllers (`#626 `_) +* Steering odometry library and controllers (`#484 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković + +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/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..5d662c1fec --- /dev/null +++ b/bicycle_steering_controller/CMakeLists.txt @@ -0,0 +1,105 @@ +cmake_minimum_required(VERSION 3.16) +project(bicycle_steering_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(bicycle_steering_controller_parameters + src/bicycle_steering_controller.yaml +) + +add_library( + bicycle_steering_controller + SHARED + src/bicycle_steering_controller.cpp +) +target_compile_features(bicycle_steering_controller PUBLIC cxx_std_17) +target_include_directories(bicycle_steering_controller PUBLIC + "$" + "$") +target_link_libraries(bicycle_steering_controller PUBLIC + bicycle_steering_controller_parameters) +ament_target_dependencies(bicycle_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(bicycle_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface bicycle_steering_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock(test_load_bicycle_steering_controller + test/test_load_bicycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml + ) + ament_target_dependencies(test_load_bicycle_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_bicycle_steering_controller test/test_bicycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml) + target_include_directories(test_bicycle_steering_controller PRIVATE include) + target_link_libraries(test_bicycle_steering_controller bicycle_steering_controller) + ament_target_dependencies( + test_bicycle_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_bicycle_steering_controller_preceeding test/test_bicycle_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_preceeding_params.yaml) + target_include_directories(test_bicycle_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_bicycle_steering_controller_preceeding bicycle_steering_controller) + ament_target_dependencies( + test_bicycle_steering_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/bicycle_steering_controller +) + +install( + TARGETS bicycle_steering_controller bicycle_steering_controller_parameters + EXPORT export_bicycle_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_bicycle_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/bicycle_steering_controller/bicycle_steering_controller.xml b/bicycle_steering_controller/bicycle_steering_controller.xml new file mode 100644 index 0000000000..644c8840fa --- /dev/null +++ b/bicycle_steering_controller/bicycle_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Bicycle kinematics with one traction and one steering joint. + + + diff --git a/bicycle_steering_controller/doc/userdoc.rst b/bicycle_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..a720ff887d --- /dev/null +++ b/bicycle_steering_controller/doc/userdoc.rst @@ -0,0 +1,23 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/bicycle_steering_controller/doc/userdoc.rst + +.. _bicycle_steering_controller_userdoc: + +bicycle_steering_controller +============================= + +This controller implements the kinematics with two axes and two wheels, where the wheel on one axis is fixed (traction/drive), and the wheel on the other axis is steerable. + +The controller expects to have one commanding joint for traction, and one commanding joint for steering. +If your Ackermann steering vehicle uses differentials on axes, then you should probably use this controller since you can command only one traction velocity and steering angle for virtual wheels in the middle of the axes. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +For an exemplary parameterization see the ``test`` folder of the controller's package. + +.. generate_parameter_library_details:: ../src/bicycle_steering_controller.yaml diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp new file mode 100644 index 0000000000..1b3e050a37 --- /dev/null +++ b/bicycle_steering_controller/include/bicycle_steering_controller/bicycle_steering_controller.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ +#define BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ + +#include + +#include "bicycle_steering_controller/visibility_control.h" +#include "bicycle_steering_controller_parameters.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +namespace bicycle_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_WHEEL = 0; +static constexpr size_t STATE_STEER_AXIS = 1; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_WHEEL = 0; +static constexpr size_t CMD_STEER_WHEEL = 1; + +static constexpr size_t NR_STATE_ITFS = 2; +static constexpr size_t NR_CMD_ITFS = 2; +static constexpr size_t NR_REF_ITFS = 2; + +class BicycleSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + BicycleSteeringController(); + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() override; + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() + override; + +protected: + std::shared_ptr bicycle_param_listener_; + bicycle_steering_controller::Params bicycle_params_; +}; +} // namespace bicycle_steering_controller + +#endif // BICYCLE_STEERING_CONTROLLER__BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h new file mode 100644 index 0000000000..b076a00215 --- /dev/null +++ b/bicycle_steering_controller/include/bicycle_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef BICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define BICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // BICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml new file mode 100644 index 0000000000..e77fd0e0bb --- /dev/null +++ b/bicycle_steering_controller/package.xml @@ -0,0 +1,36 @@ + + + + bicycle_steering_controller + 3.11.0 + Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. + Apache License 2.0 + Bence Magyar + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp new file mode 100644 index 0000000000..5f013d7d7a --- /dev/null +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "bicycle_steering_controller/bicycle_steering_controller.hpp" + +namespace bicycle_steering_controller +{ +BicycleSteeringController::BicycleSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} + +void BicycleSteeringController::initialize_implementation_parameter_listener() +{ + bicycle_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() +{ + bicycle_params_ = bicycle_param_listener_->get_params(); + + const double wheelbase = bicycle_params_.wheelbase; + const double front_wheel_radius = bicycle_params_.front_wheel_radius; + const double rear_wheel_radius = bicycle_params_.rear_wheel_radius; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheel_radius, wheelbase); + } + else + { + odometry_.set_wheel_params(front_wheel_radius, wheelbase); + } + + odometry_.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + + RCLCPP_INFO(get_node()->get_logger(), "bicycle odometry configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double 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)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position(rear_wheel_value, steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity(rear_wheel_value, steer_position, period.seconds()); + } + } + } + return true; +} +} // namespace bicycle_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + bicycle_steering_controller::BicycleSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.yaml b/bicycle_steering_controller/src/bicycle_steering_controller.yaml new file mode 100644 index 0000000000..c40e27ef96 --- /dev/null +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -0,0 +1,24 @@ +bicycle_steering_controller: + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheel. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + front_wheel_radius: + { + type: double, + default_value: 0.0, + description: "Front wheel radius.", + read_only: false, + } + + rear_wheel_radius: + { + type: double, + default_value: 0.0, + description: "Rear wheel radius.", + read_only: false, + } diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml new file mode 100644 index 0000000000..a2a6c6508b --- /dev/null +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -0,0 +1,15 @@ +test_bicycle_steering_controller: + ros__parameters: + + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_wheel_joint] + front_wheels_names: [steering_axis_joint] + + wheelbase: 3.24644 + front_wheel_radius: 0.45 + rear_wheel_radius: 0.45 diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..39ffeed878 --- /dev/null +++ b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml @@ -0,0 +1,16 @@ +test_bicycle_steering_controller: + ros__parameters: + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_wheel_joint] + front_wheels_names: [pid_controller/steering_axis_joint] + rear_wheels_state_names: [rear_wheel_joint] + front_wheels_state_names: [steering_axis_joint] + + wheelbase: 3.24644 + front_wheel_radius: 0.45 + rear_wheel_radius: 0.45 diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp new file mode 100644 index 0000000000..06b0c7e846 --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -0,0 +1,266 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_bicycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class BicycleSteeringControllerTest +: public BicycleSteeringControllerFixture +{ +}; + +TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->bicycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_); + ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); +} + +TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + + // check ref itfsTIME + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +TEST_F(BicycleSteeringControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(BicycleSteeringControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(BicycleSteeringControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(BicycleSteeringControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(BicycleSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(BicycleSteeringControllerTest, publish_status_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); +} + +TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_EQ(msg.linear_velocity_command[0], 1.1); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); + + publish_commands(); + ASSERT_TRUE(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); + 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.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp new file mode 100644 index 0000000000..065f9e1a0d --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -0,0 +1,284 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_BICYCLE_STEERING_CONTROLLER_HPP_ +#define TEST_BICYCLE_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "bicycle_steering_controller/bicycle_steering_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using bicycle_steering_controller::STATE_STEER_AXIS; +using bicycle_steering_controller::STATE_TRACTION_WHEEL; + +// name constants for command interfaces +using bicycle_steering_controller::CMD_STEER_WHEEL; +using bicycle_steering_controller::CMD_TRACTION_WHEEL; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; + +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableBicycleSteeringController +: public bicycle_steering_controller::BicycleSteeringController +{ + FRIEND_TEST(BicycleSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(BicycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(BicycleSteeringControllerTest, activate_success); + FRIEND_TEST(BicycleSteeringControllerTest, update_success); + FRIEND_TEST(BicycleSteeringControllerTest, deactivate_success); + FRIEND_TEST(BicycleSteeringControllerTest, reactivate_success); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic); + FRIEND_TEST(BicycleSteeringControllerTest, test_update_logic_chained); + FRIEND_TEST(BicycleSteeringControllerTest, publish_status_success); + FRIEND_TEST(BicycleSteeringControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + 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; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return bicycle_steering_controller::BicycleSteeringController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerReferenceMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool 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); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class BicycleSteeringControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_bicycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_bicycle_steering_controller/controller_state", 10, subs_callback); + + // 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); + // 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)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + 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 + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_wheel_joint"}; + std::vector front_wheels_names_ = {"steering_axis_joint"}; + std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; + + std::vector rear_wheels_preceeding_names_ = {"pid_controller/rear_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], front_wheels_preceeding_names_[0]}; + + double wheelbase_ = 3.24644; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {3.3, 0.5}; + std::array joint_command_values_ = {1.1, 2.2}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_BICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..875910ba23 --- /dev/null +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -0,0 +1,95 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_bicycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class BicycleSteeringControllerTest +: public BicycleSteeringControllerFixture +{ +}; + +TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->bicycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_); + ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); +} + +TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + + EXPECT_EQ( + command_intefaces.names[CMD_STEER_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp new file mode 100644 index 0000000000..955feb33c5 --- /dev/null +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -0,0 +1,48 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadBicycleSteeringController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 272656ce20..fddec6b53d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- +* removed duplicated previous_publish_timestamp\_ increment by publish_period\_ in diff_drive_controller.cpp (`#644 `_) +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Sai Kishor Kothakota, Jules CARPENTIER + +3.9.0 (2023-05-28) +------------------ +* Use generate_parameter_library for all params (`#601 `_) +* Use branch name substitution for all links (`#618 `_) +* Fix compilation warnings (`#621 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich, Noel Jiménez García, Mathias Lüdtke + +3.8.0 (2023-05-14) +------------------ +* Clear registered handles of DiffDriveController on deactivate (`#596 `_) +* Contributors: Noel Jiménez García + 3.7.0 (2023-05-02) ------------------ * Fix wrong publish timestamp initialization (`#585 `_) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 1077982380..b944ff1e88 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(diff_drive_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst index 1ef47bd975..6bd682af26 100644 --- a/diff_drive_controller/doc/userdoc.rst +++ b/diff_drive_controller/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/diff_drive_controller/doc/userdoc.rst + .. _diff_drive_controller_userdoc: diff_drive_controller @@ -70,9 +72,9 @@ Services Parameters ------------ -Check `parameter definition file for details `_. +Check `parameter definition file for details `_. -Note that the documentation on parameters for joint limits can be found in `their header file `_. +Note that the documentation on parameters for joint limits can be found in `their header file `_. Those parameters are: linear.x [JointLimits structure] diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 0fdbb3a8a5..f7d2f2f361 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 3.7.0 + 3.11.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 22b9acd829..20736fda09 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -167,8 +167,8 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= params_.wheels_per_side; - right_feedback_mean /= params_.wheels_per_side; + left_feedback_mean /= static_cast(params_.wheels_per_side); + right_feedback_mean /= static_cast(params_.wheels_per_side); if (params_.position_feedback) { @@ -194,7 +194,7 @@ controller_interface::return_type DiffDriveController::update( should_publish = true; } } - catch (const std::runtime_error) + catch (const std::runtime_error &) { // Handle exceptions when the time source changes and initialize publish timestamp previous_publish_timestamp_ = time; @@ -203,8 +203,6 @@ controller_interface::return_type DiffDriveController::update( if (should_publish) { - previous_publish_timestamp_ += publish_period_; - if (realtime_odometry_publisher_->trylock()) { auto & odometry_message = realtime_odometry_publisher_->msg_; @@ -302,10 +300,9 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); - cmd_vel_timeout_ = std::chrono::milliseconds{ - static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; - publish_limited_velocity_ = get_node()->get_parameter("publish_limited_velocity").as_bool(); - use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; + publish_limited_velocity_ = params_.publish_limited_velocity; + use_stamped_vel_ = params_.use_stamped_vel; limiter_linear_ = SpeedLimiter( params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, @@ -414,7 +411,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_message.child_frame_id = controller_namespace + base_frame_id; // limit the publication on the topics /odom and /tf - publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); + publish_rate_ = params_.publish_rate; publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); // initialize odom values zeros @@ -481,6 +478,13 @@ controller_interface::CallbackReturn DiffDriveController::on_deactivate( const rclcpp_lifecycle::State &) { subscriber_is_active_ = false; + if (!is_halted) + { + halt(); + is_halted = true; + } + registered_left_wheel_handles_.clear(); + registered_right_wheel_handles_.clear(); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 236f34e9a2..adf4e79afc 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -48,12 +48,12 @@ 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 - */ + * @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( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 1c6b1dbd9e..763381c065 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/controllers_index.rst + .. _controllers: ################# @@ -20,7 +22,7 @@ The controllers' namespaces are commanding the following command interface types - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT`` - ... -.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -40,13 +42,18 @@ Available Controllers .. toctree:: :titlesonly: + Ackermann Steering Controller <../ackermann_steering_controller/doc/userdoc.rst> Admittance Controller <../admittance_controller/doc/userdoc.rst> + Bicycle Steering Controller <../bicycle_steering_controller/doc/userdoc.rst> Differential Drive Controller <../diff_drive_controller/doc/userdoc.rst> Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> + Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Position Controllers <../position_controllers/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> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index dcf577e131..65ffc6795b 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -1,9 +1,11 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/writing_new_controller.rst + .. _writing_new_controllers: Writing a new controller ======================== -In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. +In this framework controllers are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new controller. 1. **Preparing package** diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 767184b5d3..b21b0c547d 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-05-14) +------------------ + 3.7.0 (2023-05-02) ------------------ diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index 340bb19825..ad97690655 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(effort_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index 000bfcf26c..74caf63391 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/effort_controllers/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/effort_controllers/doc/userdoc.rst + .. _effort_controllers_userdoc: effort_controllers diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 02fc8594c0..567dc00741 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 3.7.0 + 3.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 672d2b53ca..de8adc57f1 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Broadcaster parameters (`#650 `_) +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: Christoph Fröhlich, gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-05-14) +------------------ + 3.7.0 (2023-05-02) ------------------ diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 207e978c10..2af5500e21 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(force_torque_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS @@ -49,11 +49,13 @@ pluginlib_export_plugin_description_file( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_load_force_torque_sensor_broadcaster + add_rostest_with_parameters_gmock(test_load_force_torque_sensor_broadcaster test/test_load_force_torque_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/force_torque_sensor_broadcaster_params.yaml) + target_include_directories(test_load_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_force_torque_sensor_broadcaster force_torque_sensor_broadcaster ) @@ -63,9 +65,10 @@ if(BUILD_TESTING) ros2_control_test_assets ) - ament_add_gmock(test_force_torque_sensor_broadcaster + add_rostest_with_parameters_gmock(test_force_torque_sensor_broadcaster test/test_force_torque_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/force_torque_sensor_broadcaster_params.yaml) + target_include_directories(test_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_force_torque_sensor_broadcaster force_torque_sensor_broadcaster ) diff --git a/force_torque_sensor_broadcaster/doc/userdoc.rst b/force_torque_sensor_broadcaster/doc/userdoc.rst index de8a1bd7c6..053723e8f0 100644 --- a/force_torque_sensor_broadcaster/doc/userdoc.rst +++ b/force_torque_sensor_broadcaster/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/force_torque_sensor_broadcaster/doc/userdoc.rst + .. _force_torque_sensor_broadcaster_userdoc: Force Torque Sensor Broadcaster diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index f7ab4d61ab..a363551958 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 3.7.0 + 3.11.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 9687f9dfee..9b570d353f 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -72,12 +72,6 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (params_.frame_id.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); - return controller_interface::CallbackReturn::ERROR; - } - if (!params_.sensor_name.empty()) { force_torque_sensor_ = std::make_unique( diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 059d5ab9a1..68a85d9d8e 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -3,6 +3,9 @@ force_torque_sensor_broadcaster: type: string, default_value: "", description: "Sensor's frame_id in which values are published.", + validation: { + not_empty<>: null + } } sensor_name: { type: string, diff --git a/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml b/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml new file mode 100644 index 0000000000..58cc1c6b16 --- /dev/null +++ b/force_torque_sensor_broadcaster/test/force_torque_sensor_broadcaster_params.yaml @@ -0,0 +1,4 @@ +test_force_torque_sensor_broadcaster: + ros__parameters: + + frame_id: "fts_sensor_frame" diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 74e2b15da0..b88266712b 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 @@ -39,9 +39,9 @@ constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace -void ForceTorqueSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +void ForceTorqueSensorBroadcasterTest::SetUpTestCase() {} -void ForceTorqueSensorBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } +void ForceTorqueSensorBroadcasterTest::TearDownTestCase() {} void ForceTorqueSensorBroadcasterTest::SetUp() { @@ -106,29 +106,6 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet) ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } -TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_FrameId_NotSet) -{ - SetUpFTSBroadcaster(); - - // set the 'interface_names' - fts_broadcaster_->get_node()->set_parameter({"interface_names.force.x", "fts_sensor/force.x"}); - fts_broadcaster_->get_node()->set_parameter({"interface_names.torque.z", "fts_sensor/torque.z"}); - - // configure failed, 'frame_id' parameter not set - ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - -TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_FrameId_NotSet) -{ - SetUpFTSBroadcaster(); - - // set the 'sensor_name' - fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - - // configure failed, 'frame_id' parameter not set - ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set) { SetUpFTSBroadcaster(); @@ -315,3 +292,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index a9ca5e88fc..0c269d6a31 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -28,8 +28,6 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -43,6 +41,13 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) "test_force_torque_sensor_broadcaster", "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); + return result; } diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index bf3b24ab82..796b01c03f 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Sai Kishor Kothakota + +3.9.0 (2023-05-28) +------------------ +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-05-14) +------------------ + 3.7.0 (2023-05-02) ------------------ diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 46189fe5ac..15ffe09750 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(forward_command_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/forward_command_controller/doc/userdoc.rst b/forward_command_controller/doc/userdoc.rst index 8292bdc1b3..ee0af57d7b 100644 --- a/forward_command_controller/doc/userdoc.rst +++ b/forward_command_controller/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/forward_command_controller/doc/userdoc.rst + .. _forward_command_controller_userdoc: forward_command_controller @@ -13,3 +15,18 @@ Hardware interface type ----------------------- This controller can be used for every type of command interface. + +Parameters +------------ + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +forward_command_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. generate_parameter_library_details:: ../src/forward_command_controller_parameters.yaml + +multi_interface_forward_command_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. generate_parameter_library_details:: ../src/multi_interface_forward_command_controller_parameters.yaml diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 3e153d7e2e..d60245f328 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 @@ -87,8 +87,8 @@ class ForwardControllersBase : public controller_interface::ControllerInterface * * It is expected that error handling of exceptions is done. * - * \returns controller_interface::CallbackReturn::SUCCESS if parameters are successfully read and their values are - * allowed, controller_interface::CallbackReturn::ERROR otherwise. + * \returns controller_interface::CallbackReturn::SUCCESS if parameters are successfully read and + * their values are allowed, controller_interface::CallbackReturn::ERROR otherwise. */ virtual controller_interface::CallbackReturn read_parameters() = 0; diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 4441a019ab..0069f28315 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 3.7.0 + 3.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 697e42d671..b39294f36c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -206,7 +206,7 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) // update successful, command received ASSERT_EQ( - controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(100000000), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // check joint commands have been modified diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 0cada04859..c629b36ba8 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 @@ -205,7 +205,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) // update successful, command received ASSERT_EQ( - controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(100000000), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // check command in handle was set @@ -289,7 +289,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes // update successful, command received ASSERT_EQ( - controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(100000000), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); // check command in handle was set diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index b7378fa760..e4cb8cad5e 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Fix compilation warnings (`#621 `_) +* Contributors: Noel Jiménez García, Mathias Lüdtke + +3.8.0 (2023-05-14) +------------------ + 3.7.0 (2023-05-02) ------------------ diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index e8fd1e5d7e..8edaaf6386 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -7,7 +7,7 @@ if(APPLE OR WIN32) endif() if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/gripper_controllers/doc/userdoc.rst b/gripper_controllers/doc/userdoc.rst new file mode 100644 index 0000000000..28262e90f9 --- /dev/null +++ b/gripper_controllers/doc/userdoc.rst @@ -0,0 +1,14 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/gripper_controllers/doc/userdoc.rst + +.. _gripper_controllers_userdoc: + +Gripper Action Controller +-------------------------------- + +Controller for executing a gripper command action for simple single-dof grippers. + +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/gripper_controllers/package.xml b/gripper_controllers/package.xml index 4d53c64d7f..646399f14f 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 3.7.0 + 3.11.0 The gripper_controllers package Bence Magyar diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 3e9750a603..350e551cb8 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -30,6 +30,9 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; using hardware_interface::StateInterface; +namespace +{ + // subclassing and friending so we can access member variables class FriendGripperController : public gripper_action_controller::GripperActionController @@ -62,4 +65,6 @@ class GripperControllerTest : public ::testing::Test CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &joint_commands_[0]}; }; +} // anonymous namespace + #endif // TEST_GRIPPER_CONTROLLERS_HPP_ diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 9049c11f6a..5252da9c44 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Broadcaster parameters (`#650 `_) +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-05-14) +------------------ + 3.7.0 (2023-05-02) ------------------ diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 18d883503b..3b09d9e72e 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(imu_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS @@ -49,11 +49,13 @@ pluginlib_export_plugin_description_file( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_load_imu_sensor_broadcaster + add_rostest_with_parameters_gmock(test_load_imu_sensor_broadcaster test/test_load_imu_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/imu_sensor_broadcaster_params.yaml) + target_include_directories(test_load_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_imu_sensor_broadcaster imu_sensor_broadcaster ) @@ -63,9 +65,10 @@ if(BUILD_TESTING) ros2_control_test_assets ) - ament_add_gmock(test_imu_sensor_broadcaster + add_rostest_with_parameters_gmock(test_imu_sensor_broadcaster test/test_imu_sensor_broadcaster.cpp - ) + ${CMAKE_CURRENT_SOURCE_DIR}/test/imu_sensor_broadcaster_params.yaml) + target_include_directories(test_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_imu_sensor_broadcaster imu_sensor_broadcaster ) diff --git a/imu_sensor_broadcaster/doc/userdoc.rst b/imu_sensor_broadcaster/doc/userdoc.rst index 90704c8504..3b8ae172db 100644 --- a/imu_sensor_broadcaster/doc/userdoc.rst +++ b/imu_sensor_broadcaster/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/imu_sensor_broadcaster/doc/userdoc.rst + .. _imu_sensor_broadcaster_userdoc: IMU Sensor Broadcaster @@ -9,10 +11,6 @@ The controller is a wrapper around ``IMUSensor`` semantic component (see ``contr Parameters ^^^^^^^^^^^ -frame_id (mandatory) - Frame in which the output message will be published. +This controller uses the `generate_parameter_library `_ to handle its parameters. -sensor_name (mandatory) - Defines sensor name used as prefix for its interfaces. - Interface names are: /orientation.x, ..., /angular_velocity.x, ..., - /linear_acceleration.x. +.. generate_parameter_library_details:: ../src/imu_sensor_broadcaster_parameters.yaml diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index b5e9e6c9f8..993bec7ab4 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 3.7.0 + 3.11.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 3e2d574f0f..04a28e5368 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -44,17 +44,6 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); - if (params_.sensor_name.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter has to be specified."); - return CallbackReturn::ERROR; - } - - if (params_.frame_id.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); - return CallbackReturn::ERROR; - } imu_sensor_ = std::make_unique( semantic_components::IMUSensor(params_.sensor_name)); diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml index c0daba9f11..d45bf8583b 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml @@ -2,12 +2,20 @@ imu_sensor_broadcaster: sensor_name: { type: string, default_value: "", - description: "Name of the sensor used as prefix for interfaces if there are no individual interface names defined.", + description: "Defines sensor name used as prefix for its interfaces. + Interface names are: ``/orientation.x, ..., /angular_velocity.x, ..., + /linear_acceleration.x.``", + validation: { + not_empty<>: null + } } frame_id: { type: string, default_value: "", description: "Sensor's frame_id in which values are published.", + validation: { + not_empty<>: null + } } static_covariance_orientation: { type: double_array, diff --git a/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml b/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml new file mode 100644 index 0000000000..c1c660d2c4 --- /dev/null +++ b/imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml @@ -0,0 +1,5 @@ +test_imu_sensor_broadcaster: + ros__parameters: + + sensor_name: "imu_sensor" + frame_id: "imu_sensor_frame" diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 31c8d9e2f0..8358088b1d 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -40,9 +40,9 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace -void IMUSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +void IMUSensorBroadcasterTest::SetUpTestCase() {} -void IMUSensorBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } +void IMUSensorBroadcasterTest::TearDownTestCase() {} void IMUSensorBroadcasterTest::SetUp() { @@ -102,36 +102,6 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & ASSERT_TRUE(subscription->take(imu_msg, msg_info)); } -TEST_F(IMUSensorBroadcasterTest, SensorName_FrameId_NotSet) -{ - SetUpIMUBroadcaster(); - - // configure failed - ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - -TEST_F(IMUSensorBroadcasterTest, SensorName_NotSet) -{ - SetUpIMUBroadcaster(); - - // set the 'frame_id' - imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); - - // configure failed, 'sensor_name' parameter not set - ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - -TEST_F(IMUSensorBroadcasterTest, FrameId_NotSet) -{ - SetUpIMUBroadcaster(); - - // set the 'sensor_name' - imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); - - // configure failed, 'frame_id' parameter not set - ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); -} - TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) { SetUpIMUBroadcaster(); @@ -208,3 +178,12 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success) EXPECT_EQ(imu_msg.linear_acceleration_covariance[i], 0.0); } } + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index e7cfd1bcf7..f4e6105ed6 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -28,8 +28,6 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -42,6 +40,13 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) cm.load_controller( "test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); rclcpp::shutdown(); + return result; } diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 3812d61ac3..9337cd24b2 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Use branch name substitution for all links (`#618 `_) +* [JTC] Fix deprecated header (`#610 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-05-14) +------------------ + 3.7.0 (2023-05-02) ------------------ diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 018adef23a..cadc96b4e3 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(joint_state_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index fbdb439992..c9164ec723 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_state_broadcaster/doc/userdoc.rst + .. _joint_state_broadcaster_userdoc: joint_state_broadcaster diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 5ad73b4ea0..da7683d69e 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 3.7.0 + 3.11.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 74a19c9ed0..3c2192d40e 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -24,8 +24,8 @@ #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/qos_event.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rcpputils/split.hpp" diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index fbb24978b7..354516fbb2 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* jtc: fix minor typo in traj validation error msg (`#674 `_) +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: G.A. vd. Hoorn, gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Sai Kishor Kothakota + +3.9.0 (2023-05-28) +------------------ +* Use branch name substitution for all links (`#618 `_) +* [JTC] Fix deprecated header (`#610 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-05-14) +------------------ +* [JTC] Import docs from wiki.ros.org (`#566 `_) +* Contributors: Christoph Fröhlich + 3.7.0 (2023-05-02) ------------------ * Fix JTC from immediately returning success (`#565 `_) diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 9664887f4a..0552e0bc86 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(joint_trajectory_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 1ba6257a5a..7529a60918 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -1,14 +1,26 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/doc/userdoc.rst + .. _joint_trajectory_controller_userdoc: joint_trajectory_controller =========================== -Controller for executing joint-space trajectories on a group of joints. Trajectories are specified as a set of waypoints to be reached at specific time instants, which the controller attempts to execute as well as the mechanism allows. Waypoints consist of positions, and optionally velocities and accelerations. +Controller for executing joint-space trajectories on a group of joints. +The controller interpolates in time between the points so that their distance can be arbitrary. +Even trajectories with only one point are accepted. +Trajectories are specified as a set of waypoints to be reached at specific time instants, +which the controller attempts to execute as well as the mechanism allows. +Waypoints consist of positions, and optionally velocities and accelerations. + + +*Parts of this documentation were originally published in the ROS 1 wiki under the* `CC BY 3.0 license `_. *Citations are given in the respective section, but were adapted for the ROS 2 implementation.* [#f1]_ [#f2]_ -Trajectory representation -------------------------- +Trajectory representation [#f1]_ +--------------------------------- -The controller is templated to work with multiple trajectory representations. By default, a spline interpolator is provided, but it's possible to support other representations. The spline interpolator uses the following interpolation strategies depending on the waypoint specification: +Trajectories are represented internally with ``trajectory_msgs/msg/JointTrajectory`` data structure. +By default, a spline interpolator is provided, but it's possible to support other representations. +The spline interpolator uses the following interpolation strategies depending on the waypoint specification: * Linear: Only position is specified. Guarantees continuity at the position level. Discouraged because it yields trajectories with discontinuous velocities at the waypoints. @@ -16,10 +28,27 @@ The controller is templated to work with multiple trajectory representations. By * Quintic: Position, velocity and acceleration are specified: Guarantees continuity at the acceleration level. -Hardware interface type ------------------------ +Hardware interface type [#f1]_ +------------------------------- + +Currently joints with position, velocity, acceleration, and effort interfaces are supported. The joints can have one or more command interfaces, where the following control laws are applied at the same time: + +* For command interfaces ``position``, the desired positions are simply forwarded to the joints, +* For command interfaces ``acceleration``, desired accelerations are simply forwarded to the joints. +* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop (:ref:`parameters`). + +This leads to the the following allowed combinations of command and state interfaces: + +* With command interface ``position``, there are no restrictions for state interfaces. +* With command interface ``velocity``: + + * if command interface ``velocity`` is the only one, state interfaces must include ``position, velocity`` . + * no restrictions otherwise. + +* With command interface ``effort``, state interfaces must include ``position, velocity``. +* With command interface ``acceleration``, there are no restrictions for state interfaces. -The controller is templated to work with multiple hardware interface types. Currently joints with position, velocity and effort interfaces are supported. For position-controlled joints, desired positions are simply forwarded to the joints; while for velocity (effort) joints, the position+velocity trajectory following error is mapped to velocity (effort) commands through a PID loop. Example controller configurations can be found here. +Example controller configurations can be found :ref:`below `. Similarly to the trajectory representation case above, it's possible to support new hardware interfaces, or alternative mappings to an already supported interface (eg. a proxy controller for generating effort commands). @@ -32,27 +61,6 @@ Other features * Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments. -ros2_control interfaces ------------------------- - -References -^^^^^^^^^^^ -(the controller is not yet implemented as chainable controller) - -States -^^^^^^^ -The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. -Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. -Legal combinations of state interfaces are: - -* ``position`` -* ``position`` and ``velocity`` -* ``position``, ``velocity`` and ``acceleration`` -* ``effort`` - -Commands -^^^^^^^^^ - Using Joint Trajectory Controller(s) ------------------------------------ @@ -101,6 +109,17 @@ A yaml file for using it could be: goal: 0.03 +Preemption policy [#f1]_ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Only one action goal can be active at any moment, or none if the topic interface is used. Path and goal tolerances are checked only for the trajectory segments of the active goal. + +When an active action goal is preempted by another command coming from the action interface, the goal is canceled and the client is notified. + +Sending an empty trajectory message from the topic interface (not the action interface) will override the current action goal and not abort the action. + +.. _parameters: + Details about parameters ^^^^^^^^^^^^^^^^^^^^^^^^ @@ -143,8 +162,9 @@ interpolation_method (string) open_loop_control (boolean) Use controller in open-loop control mode: - + The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. - + It deactivates the feedback control, see the ``gains`` structure. + + * The controller ignores the states provided by hardware interface but using last commands as states for starting the trajectory interpolation. + * It deactivates the feedback control, see the ``gains`` structure. This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators). @@ -224,17 +244,79 @@ gains..normalize_error (bool) Default: false -ROS2 interface of the controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -~/joint_trajectory (input topic) [trajectory_msgs::msg::JointTrajectory] - Topic for commanding the controller. +.. _ROS 2 interface: + +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +References +,,,,,,,,,,,,,,,,,, + +(the controller is not yet implemented as chainable controller) + +States +,,,,,,,,,,,,,,,,,, + +The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``/``. +Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the `hardware_interface/hardware_interface_type_values.hpp `_. + +Legal combinations of state interfaces are: + +* ``position`` +* ``position`` and ``velocity`` +* ``position``, ``velocity`` and ``acceleration`` +* ``effort`` + +Commands +,,,,,,,,, + +There are two mechanisms for sending trajectories to the controller: + +* via action, see :ref:`actions ` +* via topic, see :ref:`subscriber ` + +Both use the ``trajectory_msgs/JointTrajectory`` message to specify trajectories, and require specifying values for all the controller joints (as opposed to only a subset) if ``allow_partial_joints_goal`` is not set to ``True``. -~/controller_state (output topic) [control_msgs::msg::JointTrajectoryControllerState] - Topic publishing internal states with the update-rate of the controller manager. +.. _Actions: -~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory] - Action server for commanding the controller. +Actions [#f1]_ +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +/follow_joint_trajectory [control_msgs::action::FollowJointTrajectory] + Action server for commanding the controller + + +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. +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. + +.. _Subscriber: + +Subscriber [#f1]_ +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, + +/joint_trajectory [trajectory_msgs::msg::JointTrajectory] + Topic for commanding the controller + +The topic interface is a fire-and-forget alternative. Use this interface if you don't care about execution monitoring. +The controller's path and goal tolerance specification is not used in this case, as there is no mechanism to notify the sender about tolerance violations. +Note that although some degree of monitoring is available through the ``~/query_state`` service and ``~/state`` topic it is much more cumbersome to realize than with the action interface. + + +Publishers +,,,,,,,,,,, + +/controller_state [control_msgs::msg::JointTrajectoryControllerState] + Topic publishing internal states with the update-rate of the controller manager + + +Services +,,,,,,,,,,, + +/query_state [control_msgs::srv::QueryTrajectoryState] + Query controller state at any future time Specialized versions of JointTrajectoryController (TBD in ...) @@ -246,26 +328,8 @@ The following version of the Joint Trajectory Controller are available mapping t * position_controllers::JointTrajectoryController - * Input: position, [velocity, [acceleration]] - * Output: position - -* position_velocity_controllers::JointTrajectoryController - - * Input: position, [velocity, [acceleration]] - * Output: position and velocity - -* position_velocity_acceleration_controllers::JointTrajectoryController - - * Input: position, [velocity, [acceleration]] - * Output: position, velocity and acceleration -.. * velocity_controllers::JointTrajectoryController -.. * Input: position, [velocity, [acceleration]] -.. * Output: velocity -.. TODO(anyone): would it be possible to output velocty and acceleration? -.. (to have an vel_acc_controllers) -.. * effort_controllers::JointTrajectoryController -.. * Input: position, [velocity, [acceleration]] -.. * Output: effort +.. rubric:: Footnote -(*Not implemented yet*) When using pure ``velocity`` or ``effort`` controllers a command is generated using the desired state and state error using a velocity feedforward term plus a corrective PID term. (#171) +.. [#f1] Adolfo Rodriguez: `joint_trajectory_controller `_ +.. [#f2] Adolfo Rodriguez: `Understanding trajectory replacement `_ diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index de8f060b1a..b4cb029dd9 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -123,11 +123,11 @@ SegmentTolerances get_segment_tolerances(Params const & params) * \param state_error State error to check. * \param joint_idx Joint index for the state error * \param state_tolerance State tolerance of joint to check \p state_error against. - * \param show_errors If the joint that violate its tolerance should be output to console. NOT REALTIME if true - * \return True if \p state_error fulfills \p state_tolerance. + * \param show_errors If the joint that violate its tolerance should be output to console. NOT + * REALTIME if true \return True if \p state_error fulfills \p state_tolerance. */ inline bool check_state_tolerance_per_joint( - const trajectory_msgs::msg::JointTrajectoryPoint & state_error, int joint_idx, + const trajectory_msgs::msg::JointTrajectoryPoint & state_error, size_t joint_idx, const StateTolerances & state_tolerance, bool show_errors = false) { using std::abs; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 7dac4ddbe1..230d0198f7 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -60,26 +60,29 @@ class Trajectory /// containing trajectory. /** * Sampling trajectory at given \p sample_time. - * If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or acceleration respectively. - * Deduction assumes that the provided velocity or acceleration have to be reached at the time defined in the segment. + * If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or + * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to + * be reached at the time defined in the segment. * * Specific case returns for start_segment_itr and end_segment_itr: * - Sampling before the trajectory start: * start_segment_itr = begin(), end_segment_itr = begin() * - Sampling exactly on a point of the trajectory: - * start_segment_itr = iterator where point is, end_segment_itr = iterator after start_segment_itr + * start_segment_itr = iterator where point is, end_segment_itr = iterator after + * start_segment_itr * - Sampling between points: - * start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after start_segment_itr + * start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after + * start_segment_itr * - Sampling after entire trajectory: * start_segment_itr = --end(), end_segment_itr = end() * - Sampling empty msg or before the time given in set_point_before_trajectory_msg() * return false * * \param[in] sample_time Time at which trajectory will be sampled. - * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at all. - * \param[out] expected_state Calculated new at \p sample_time. - * \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See description above. - * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See description above. + * \param[in] interpolation_method Specify whether splines, another method, or no interpolation at + * all. \param[out] expected_state Calculated new at \p sample_time. \param[out] start_segment_itr + * Iterator to the start segment for given \p sample_time. See description above. \param[out] + * end_segment_itr Iterator to the end segment for given \p sample_time. See description above. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( @@ -91,14 +94,16 @@ class Trajectory /** * Do interpolation between 2 states given a time in between their respective timestamps * - * The start and end states need not necessarily be specified all the way to the acceleration level: + * The start and end states need not necessarily be specified all the way to the acceleration + * level: * - If only \b positions are specified, linear interpolation will be used. * - If \b positions and \b velocities are specified, a cubic spline will be used. - * - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be used. + * - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be + * used. * * If start and end states have different specifications - * (eg. start is position-only, end is position-velocity), the lowest common specification will be used - * (position-only in the example). + * (eg. start is position-only, end is position-velocity), the lowest common specification will be + * used (position-only in the example). * * \param[in] time_a Time at which the segment state equals \p state_a. * \param[in] state_a State at \p time_a. @@ -153,9 +158,9 @@ class Trajectory }; /** - * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 indices. - * If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated mapping vector is - * "{2, 1}". + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 + * indices. If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated + * mapping vector is "{2, 1}". */ template inline std::vector mapping(const T & t1, const T & t2) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 00cad650c4..edf2ac91a9 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 3.7.0 + 3.11.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 07080bb1fc..4d994e26bf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -31,10 +31,10 @@ #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/parameter.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.hpp" #include "rclcpp/time.hpp" #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server_goal_handle.hpp" @@ -117,7 +117,7 @@ controller_interface::return_type JointTrajectoryController::update( } auto compute_error_for_joint = [&]( - JointTrajectoryPoint & error, int index, + JointTrajectoryPoint & error, size_t index, const JointTrajectoryPoint & current, const JointTrajectoryPoint & desired) { @@ -1299,7 +1299,7 @@ bool JointTrajectoryController::validate_trajectory_msg( { RCLCPP_ERROR( get_node()->get_logger(), - "Received trajectory with non zero time start time (%f) that ends on the past (%f)", + "Received trajectory with non-zero start time (%f) that ends in the past (%f)", trajectory_start_time.seconds(), trajectory_end_time.seconds()); return false; } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index fdcf8ceebd..714d3c8d49 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -35,13 +35,13 @@ #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/qos_event.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" @@ -445,7 +445,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) const double EPS = 1e-6; /** * @brief check if position error of revolute joints are normalized if not configured so -*/ + */ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) { rclcpp::executors::MultiThreadedExecutor executor; @@ -552,7 +552,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) /** * @brief check if position error of revolute joints are normalized if configured so -*/ + */ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) { rclcpp::executors::MultiThreadedExecutor executor; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 86a2a4b4c7..9855341edf 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -61,12 +61,12 @@ 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 - */ + * @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( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) @@ -295,7 +295,8 @@ class TrajectoryControllerTest : public ::testing::Test /** * delay_btwn_points - delay between each points * points - vector of trajectories. One point per controlled joint - * joint_names - names of joints, if empty, will use joint_names_ up to the number of provided points + * joint_names - names of joints, if empty, will use joint_names_ up to the number of provided + * points */ void publish( const builtin_interfaces::msg::Duration & delay_btwn_points, diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 3b04f3b06f..299b7b7de7 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-05-14) +------------------ + 3.7.0 (2023-05-02) ------------------ diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index 05714a6e46..54a4f94656 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(position_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/position_controllers/doc/userdoc.rst b/position_controllers/doc/userdoc.rst index 1e5dd53e63..f321e8a698 100644 --- a/position_controllers/doc/userdoc.rst +++ b/position_controllers/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/position_controllers/doc/userdoc.rst + .. _position_controllers_userdoc: position_controllers diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b1ca3b38b7..d2c4eaecc6 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 3.7.0 + 3.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers-not-released.foxy.repos b/ros2_controllers-not-released.foxy.repos deleted file mode 100644 index 1b3910e7e7..0000000000 --- a/ros2_controllers-not-released.foxy.repos +++ /dev/null @@ -1,6 +0,0 @@ -repositories: - ## EXAMPLE DEPENDENCY -# : -# type: git -# url: git@github.com:/.git -# version: master diff --git a/ros2_controllers.foxy.repos b/ros2_controllers.foxy.repos deleted file mode 100644 index d9a1c841a5..0000000000 --- a/ros2_controllers.foxy.repos +++ /dev/null @@ -1,13 +0,0 @@ -repositories: - ros-controls/ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: foxy - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: foxy-devel - realtime_tools: - type: git - url: https://github.com/ros-controls/realtime_tools.git - version: foxy-devel diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 3397d446e8..4ca34fc0a5 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Steering odometry library and controllers (`#484 `_) +* Contributors: Tomislav Petković + +3.8.0 (2023-05-14) +------------------ + 3.7.0 (2023-05-02) ------------------ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index b2b7fe286c..ce06ac9d71 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 3.7.0 + 3.11.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios @@ -10,7 +10,9 @@ ament_cmake + ackermann_steering_controller admittance_controller + bicycle_steering_controller diff_drive_controller effort_controllers force_torque_sensor_broadcaster @@ -19,7 +21,9 @@ joint_state_broadcaster joint_trajectory_controller position_controllers + steering_controllers_library tricycle_controller + tricycle_steering_controller velocity_controllers diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 69e4765398..f1a00b9bf4 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Removes deprecated if-branch (`#653 `_) +* Contributors: Christoph Fröhlich + +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) ------------------ diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 3fee344465..27cc252293 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 3.7.0 + 3.11.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py index 56b56c5998..4ed198515e 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 @@ -68,83 +68,59 @@ def __init__(self): self.goals = [] # List of JointTrajectoryPoint for name in goal_names: self.declare_parameter(name, descriptor=ParameterDescriptor(dynamic_typing=True)) - goal = self.get_parameter(name).value - # TODO(anyone): remove this "if" part in ROS Iron - if isinstance(goal, list): - self.get_logger().warn( - f'Goal "{name}" is defined as a list. This is deprecated. ' - "Use the following structure:\n:\n " - "positions: [joint1, joint2, joint3, ...]\n " - "velocities: [v_joint1, v_joint2, ...]\n " - "accelerations: [a_joint1, a_joint2, ...]\n " - "effort: [eff_joint1, eff_joint2, ...]" - ) + point = JointTrajectoryPoint() - if goal is None or len(goal) == 0: - raise Exception(f'Values for goal "{name}" not set!') + def get_sub_param(sub_param): + param_name = name + "." + sub_param + self.declare_parameter(param_name, [float()]) + param_value = self.get_parameter(param_name).value - float_goal = [float(value) for value in goal] + float_values = [] - point = JointTrajectoryPoint() - point.positions = float_goal - point.time_from_start = Duration(sec=4) + if len(param_value) != len(self.joints): + return [False, float_values] + + float_values = [float(value) for value in param_value] + + return [True, float_values] + + one_ok = False + + [ok, values] = get_sub_param("positions") + if ok: + point.positions = values + one_ok = True + [ok, values] = get_sub_param("velocities") + if ok: + point.velocities = values + one_ok = True + + [ok, values] = get_sub_param("accelerations") + if ok: + point.accelerations = values + one_ok = True + + [ok, values] = get_sub_param("effort") + if ok: + point.effort = values + one_ok = True + + if one_ok: + point.time_from_start = Duration(sec=4) self.goals.append(point) + self.get_logger().info(f'Goal "{name}" has definition {point}') else: - point = JointTrajectoryPoint() - - def get_sub_param(sub_param): - param_name = name + "." + sub_param - self.declare_parameter(param_name, [float()]) - param_value = self.get_parameter(param_name).value - - float_values = [] - - if len(param_value) != len(self.joints): - return [False, float_values] - - float_values = [float(value) for value in param_value] - - return [True, float_values] - - one_ok = False - - [ok, values] = get_sub_param("positions") - if ok: - point.positions = values - one_ok = True - - [ok, values] = get_sub_param("velocities") - if ok: - point.velocities = values - one_ok = True - - [ok, values] = get_sub_param("accelerations") - if ok: - point.accelerations = values - one_ok = True - - [ok, values] = get_sub_param("effort") - if ok: - point.effort = values - one_ok = True - - if one_ok: - point.time_from_start = Duration(sec=4) - self.goals.append(point) - self.get_logger().info(f'Goal "{name}" has definition {point}') - - else: - self.get_logger().warn( - f'Goal "{name}" definition is wrong. This goal will not be used. ' - "Use the following structure: \n:\n " - "positions: [joint1, joint2, joint3, ...]\n " - "velocities: [v_joint1, v_joint2, ...]\n " - "accelerations: [a_joint1, a_joint2, ...]\n " - "effort: [eff_joint1, eff_joint2, ...]" - ) + self.get_logger().warn( + f'Goal "{name}" definition is wrong. This goal will not be used. ' + "Use the following structure: \n:\n " + "positions: [joint1, joint2, joint3, ...]\n " + "velocities: [v_joint1, v_joint2, ...]\n " + "accelerations: [a_joint1, a_joint2, ...]\n " + "effort: [eff_joint1, eff_joint2, ...]" + ) if len(self.goals) < 1: self.get_logger().error("No valid goal found. Exiting...") diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index 3cdf9a76ef..35f7f7b942 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="3.7.0", + version="3.11.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 b9c0b090d0..648b093d2f 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,25 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- +* Adapted rqt_jtc to newest control_msgs for jtc (`#643 `_) +* Contributors: gwalck + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ +* switch from dash to underscore in setup.cfg (`#595 `_) +* Contributors: Alex Moriarty + 3.7.0 (2023-05-02) ------------------ diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 86196cac41..c44049b9ff 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 - 3.7.0 + 3.11.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 7e8ec4948e..99f43e125e 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 @@ -80,8 +80,8 @@ class JointTrajectoryController(Plugin): the following requisites: - The controller type contains the C{JointTrajectoryController} substring, e.g., C{position_controllers/JointTrajectoryController} - - The controller exposes the C{command} and C{state} topics in its - ROS interface. + - The controller exposes the C{command} and C{controller_state} topics + in its ROS interface. Additionally, there must be a URDF loaded with a valid joint limit specification, namely position (when applicable) and velocity limits. @@ -252,10 +252,10 @@ def _update_jtc_list(self): def _on_speed_scaling_change(self, val): self._speed_scale = val / self._speed_scaling_widget.slider.maximum() - def _on_joint_state_change(self, actual_pos): - assert len(actual_pos) == len(self._joint_pos) - for name in actual_pos.keys(): - self._joint_pos[name]["position"] = actual_pos[name] + def _on_joint_state_change(self, current_pos): + assert len(current_pos) == len(self._joint_pos) + for name in current_pos.keys(): + self._joint_pos[name]["position"] = current_pos[name] def _on_cm_change(self, cm_ns): self._cm_ns = cm_ns @@ -289,11 +289,11 @@ def _on_jtc_enabled(self, val): self._speed_scaling_widget.setEnabled(val) if val: - # Widgets send desired position commands to controller + # Widgets send reference position commands to controller self._update_act_pos_timer.stop() self._update_cmd_timer.start() else: - # Controller updates widgets with actual position + # Controller updates widgets with feedback position self._update_cmd_timer.stop() self._update_act_pos_timer.start() @@ -332,7 +332,7 @@ def _load_jtc(self): # Setup ROS interfaces jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name) - state_topic = jtc_ns + "/state" + state_topic = jtc_ns + "/controller_state" cmd_topic = jtc_ns + "/joint_trajectory" self._state_sub = self._node.create_subscription( JointTrajectoryControllerState, state_topic, self._state_cb, 1 @@ -404,12 +404,12 @@ def _unregister_executor(self): self._executor = None def _state_cb(self, msg): - actual_pos = {} + current_pos = {} for i in range(len(msg.joint_names)): joint_name = msg.joint_names[i] - joint_pos = msg.actual.positions[i] - actual_pos[joint_name] = joint_pos - self.jointStateChanged.emit(actual_pos) + joint_pos = msg.feedback.positions[i] + current_pos[joint_name] = joint_pos + self.jointStateChanged.emit(current_pos) def _update_single_cmd_cb(self, val, name): self._joint_pos[name]["command"] = val diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index f04123ec65..5610eff43c 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="3.7.0", + version="3.11.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst new file mode 100644 index 0000000000..596f1fe9d3 --- /dev/null +++ b/steering_controllers_library/CHANGELOG.rst @@ -0,0 +1,146 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package steering_controllers_library +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + +3.10.1 (2023-06-06) +------------------- +* Second round of dependencies fix (`#655 `_) +* Contributors: Bence Magyar + +3.10.0 (2023-06-04) +------------------- +* Remove unnecessary include (`#645 `_) +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + +3.9.0 (2023-05-28) +------------------ +* Fix sphinx for steering odometry library/controllers (`#626 `_) +* Steering odometry library and controllers (`#484 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković + +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/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt new file mode 100644 index 0000000000..b64ea204a8 --- /dev/null +++ b/steering_controllers_library/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 3.16) +project(steering_controllers_library LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs + ackermann_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(steering_controllers_library_parameters + src/steering_controllers_library.yaml +) + +add_library( + steering_controllers_library + SHARED + src/steering_controllers_library.cpp + src/steering_odometry.cpp +) +target_compile_features(steering_controllers_library PUBLIC cxx_std_17) +target_include_directories(steering_controllers_library PUBLIC + "$" + "$") +target_link_libraries(steering_controllers_library PUBLIC + steering_controllers_library_parameters) +ament_target_dependencies(steering_controllers_library PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(steering_controllers_library PRIVATE "STEERING_CONTROLLERS_BUILDING_DLL") + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock( + test_steering_controllers_library test/test_steering_controllers_library.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_params.yaml) + target_include_directories(test_steering_controllers_library PRIVATE include) + target_link_libraries(test_steering_controllers_library steering_controllers_library) + ament_target_dependencies( + test_steering_controllers_library + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/steering_controllers_library +) + +install( + TARGETS steering_controllers_library steering_controllers_library_parameters + EXPORT export_steering_controllers_library + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_steering_controllers_library HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst new file mode 100644 index 0000000000..df3d1529d0 --- /dev/null +++ b/steering_controllers_library/doc/userdoc.rst @@ -0,0 +1,93 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/steering_controllers_library/doc/userdoc.rst + +.. _steering_controllers_library_userdoc: + +steering_controllers_library +============================= + +Library with shared functionalities for mobile robot controllers with steering drive (2 degrees of freedom). +The library implements generic odometry and update methods and defines the main interfaces. + +Nomenclature used for the controller is used from `wikipedia `_. + +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 +Values in other components are ignored. +In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position. +Other relevant features are: + +* support for front and rear steering configurations; +* odometry publishing as Odometry and TF message; +* input command timeout based on a parameter. + +The command for the wheels are calculated using ``odometry`` library where based on concrete kinematics traction and steering commands are calculated. +Currently implemented kinematics in corresponding packages are: + +* :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. + + + +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`` + +- /position [double] # in [rad] +- /velocity [double] # in [m/s] + +``front_steering == false`` + +- /velocity [double] # in [m/s] +- /position [double] # in [rad] + +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`` + +- /position [double] # in [rad] +- / [double] # in [m] or [m/s] + +``front_steering == false`` + +- / [double] # [m] or [m/s] +- /position [double] # in [rad] + +Subscribers +,,,,,,,,,,,, +Used when controller is not in chained mode (``in_chained_mode == false``). + +- /reference [geometry_msgs/msg/TwistStamped] + If parameter ``use_stamped_vel`` is ``true``. +- /reference_unstamped [geometry_msgs/msg/Twist] + If parameter ``use_stamped_vel`` is ``false``. + +Publishers +,,,,,,,,,,, +- /odometry [nav_msgs/msg/Odometry] +- /tf_odometry [tf2_msgs/msg/TFMessage] +- /controller_state [control_msgs/msg/SteeringControllerStatus] + +Parameters +,,,,,,,,,,, +This controller uses the `generate_parameter_library `_ to handle its parameters. + +For an exemplary parameterization see the ``test`` folder of the controller's package. + +.. generate_parameter_library_details:: ../src/steering_controllers_library.yaml diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp new file mode 100644 index 0000000000..b560e2a782 --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -0,0 +1,151 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ +#define STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.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" + +// 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" + +namespace steering_controllers_library +{ +class SteeringControllersLibrary : public controller_interface::ChainableControllerInterface +{ +public: + STEERING_CONTROLLERS__VISIBILITY_PUBLIC SteeringControllersLibrary(); + + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC void + initialize_implementation_parameter_listener() = 0; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() = 0; + + virtual STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) = 0; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type + update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::return_type + update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped; + using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; + using ControllerStateMsgOdom = nav_msgs::msg::Odometry; + using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; + using AckermanControllerState = control_msgs::msg::SteeringControllerStatus; + +protected: + controller_interface::CallbackReturn set_interface_numbers( + size_t nr_state_itfs, size_t nr_cmd_itfs, size_t nr_ref_itfs); + + std::shared_ptr param_listener_; + steering_controllers_library::Params params_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms + + using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; + using ControllerStatePublisherTf = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr odom_s_publisher_; + rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; + + std::unique_ptr rt_odom_state_publisher_; + std::unique_ptr rt_tf_odom_state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + /// Odometry: + steering_odometry::SteeringOdometry odometry_; + + AckermanControllerState published_state_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr controller_s_publisher_; + std::unique_ptr controller_state_publisher_; + + // name constants for state interfaces + size_t nr_state_itfs_; + // name constants for command interfaces + size_t nr_cmd_itfs_; + // name constants for reference interfaces + size_t nr_ref_itfs_; + + // store last velocity + double last_linear_velocity_ = 0.0; + double last_angular_velocity_ = 0.0; + + std::vector rear_wheels_state_names_; + std::vector front_wheels_state_names_; + +private: + // callback for topic interface + STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( + const std::shared_ptr msg); + void reference_callback_unstamped(const std::shared_ptr msg); +}; + +} // namespace steering_controllers_library + +#endif // STEERING_CONTROLLERS_LIBRARY__STEERING_CONTROLLERS_LIBRARY_HPP_ diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp new file mode 100644 index 0000000000..3cfa056b27 --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -0,0 +1,263 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ +#define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ + +#include +#include + +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +#include "rcpputils/rolling_mean_accumulator.hpp" + +namespace steering_odometry +{ +const unsigned int BICYCLE_CONFIG = 0; +const unsigned int TRICYCLE_CONFIG = 1; +const unsigned int ACKERMANN_CONFIG = 2; +/** + * \brief The Odometry class handles odometry readings + * (2D pose and velocity with related timestamp) + */ +class SteeringOdometry +{ +public: + /** + * \brief Constructor + * Timestamp will get the current time value + * Value will be set to zero + * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + * + */ + explicit SteeringOdometry(size_t velocity_rolling_window_size = 10); + + /** + * \brief Initialize the odometry + * \param time Current time + */ + void init(const rclcpp::Time & time); + + /** + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_pos traction wheel position [rad] + * \param steer_pos Front Steer position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double traction_wheel_pos, const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel velocity [rad] + * \param left_traction_wheel_pos Left traction wheel velocity [rad] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_pos Right traction wheel position [rad] + * \param left_traction_wheel_pos Left traction wheel position [rad] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_position( + const double right_traction_wheel_pos, const double left_traction_wheel_pos, + const double right_steer_pos, const double left_steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param traction_wheel_vel Traction wheel velocity [rad/s] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double traction_wheel_vel, const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param front_steer_pos Steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest wheels position + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param right_steer_pos Right steer wheel position [rad] + * \param left_steer_pos Left steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_from_velocity( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double right_steer_pos, const double left_steer_pos, const double dt); + + /** + * \brief Updates the odometry class with latest velocity command + * \param linear Linear velocity [m/s] + * \param angular Angular velocity [rad/s] + * \param time Current time + */ + void update_open_loop(const double linear, const double angular, const double dt); + + /** + * \brief Set odometry type + * \param type odometry type + */ + void set_odometry_type(const unsigned int type); + + /** + * \brief heading getter + * \return heading [rad] + */ + double get_heading() const { return heading_; } + + /** + * \brief x position getter + * \return x position [m] + */ + double get_x() const { return x_; } + + /** + * \brief y position getter + * \return y position [m] + */ + double get_y() const { return y_; } + + /** + * \brief linear velocity getter + * \return linear velocity [m/s] + */ + double get_linear() const { return linear_; } + + /** + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ + double get_angular() const { return angular_; } + + /** + * \brief Sets the wheel parameters: radius, separation and wheelbase + */ + void set_wheel_params(double wheel_radius, double wheelbase = 0.0, 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); + + /** + * \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] + * \return Tuple of velocity commands and steering commands + */ + std::tuple, std::vector> get_commands(double Vx, double theta_dot); + + /** + * \brief Reset poses, heading, and accumulators + */ + void reset_odometry(); + +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 + */ + bool update_odometry(const double linear_velocity, const double angular, 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 + */ + void integrate_runge_kutta_2(double linear, double angular); + + /** + * \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 + */ + void integrate_exact(double linear, double angular); + + /** + * \brief Calculates steering angle from the desired translational and rotational velocity + * \param Vx Linear velocity [m] + * \param theta_dot Angular velocity [rad] + */ + double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot); + + /** + * \brief Reset linear and angular accumulators + */ + void reset_accumulators(); + + /// Current timestamp: + rclcpp::Time timestamp_; + + /// Current pose: + double x_; // [m] + double y_; // [m] + double steer_pos_; // [rad] + double heading_; // [rad] + + /// Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + /// Kinematic parameters + double wheel_track_; // [m] + double wheelbase_; // [m] + double wheel_radius_; // [m] + + /// Configuration type used for the forward kinematics + int config_type_ = -1; + + /// Previous wheel position/state [rad]: + double traction_wheel_old_pos_; + double traction_right_wheel_old_pos_; + double traction_left_wheel_old_pos_; + /// Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + rcpputils::RollingMeanAccumulator linear_acc_; + rcpputils::RollingMeanAccumulator angular_acc_; +}; +} // namespace steering_odometry + +#endif // STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ diff --git a/steering_controllers_library/include/steering_controllers_library/visibility_control.h b/steering_controllers_library/include/steering_controllers_library/visibility_control.h new file mode 100644 index 0000000000..123662031b --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/visibility_control.h @@ -0,0 +1,50 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ +#define STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((dllexport)) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __declspec(dllexport) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef STEERING_CONTROLLERS__VISIBILITY_BUILDING_DLL +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_EXPORT +#else +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC STEERING_CONTROLLERS__VISIBILITY_IMPORT +#endif +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE STEERING_CONTROLLERS__VISIBILITY_PUBLIC +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL +#else +#define STEERING_CONTROLLERS__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define STEERING_CONTROLLERS__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define STEERING_CONTROLLERS__VISIBILITY_PROTECTED __attribute__((visibility("protected"))) +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC +#define STEERING_CONTROLLERS__VISIBILITY_LOCAL +#endif +#define STEERING_CONTROLLERS__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // STEERING_CONTROLLERS_LIBRARY__VISIBILITY_CONTROL_H_ diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml new file mode 100644 index 0000000000..5918c1491e --- /dev/null +++ b/steering_controllers_library/package.xml @@ -0,0 +1,45 @@ + + + + steering_controllers_library + 3.11.0 + Package for steering robot configurations including odometry and interfaces. + Apache License 2.0 + Bence Magyar + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + ament_cmake + + generate_parameter_library + + backward_ros + control_msgs + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + rcpputils + std_srvs + tf2 + tf2_msgs + tf2_geometry_msgs + ackermann_msgs + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp new file mode 100644 index 0000000000..af2736a8a3 --- /dev/null +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -0,0 +1,558 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "steering_controllers_library/steering_controllers_library.hpp" + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace +{ // utility + +using ControllerTwistReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, + const std::shared_ptr & node) +{ + msg->header.stamp = node->now(); + msg->twist.linear.x = std::numeric_limits::quiet_NaN(); + msg->twist.linear.y = std::numeric_limits::quiet_NaN(); + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace steering_controllers_library +{ +SteeringControllersLibrary::SteeringControllersLibrary() +: controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + initialize_implementation_parameter_listener(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::set_interface_numbers( + size_t nr_state_itfs = 2, size_t nr_cmd_itfs = 2, size_t nr_ref_itfs = 2) +{ + nr_state_itfs_ = nr_state_itfs; + nr_cmd_itfs_ = nr_cmd_itfs; + nr_ref_itfs_ = nr_ref_itfs; + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); + + configure_odometry(); + + if (!params_.rear_wheels_state_names.empty()) + { + rear_wheels_state_names_ = params_.rear_wheels_state_names; + } + else + { + rear_wheels_state_names_ = params_.rear_wheels_names; + } + + if (!params_.front_wheels_state_names.empty()) + { + front_wheels_state_names_ = params_.front_wheels_state_names; + } + else + { + front_wheels_state_names_ = params_.front_wheels_names; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); + if (params_.use_stamped_vel) + { + ref_subscriber_twist_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); + } + else + { + ref_subscriber_unstamped_ = get_node()->create_subscription( + "~/reference_unstamped", subscribers_qos, + std::bind( + &SteeringControllersLibrary::reference_callback_unstamped, this, std::placeholders::_1)); + } + + std::shared_ptr msg = + std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + + try + { + // Odom state publisher + odom_s_publisher_ = get_node()->create_publisher( + "~/odometry", rclcpp::SystemDefaultsQoS()); + rt_odom_state_publisher_ = std::make_unique(odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_odom_state_publisher_->lock(); + rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); + rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; + + auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + rt_odom_state_publisher_->unlock(); + + try + { + // Tf State publisher + tf_odom_s_publisher_ = get_node()->create_publisher( + "~/tf_odometry", rclcpp::SystemDefaultsQoS()); + rt_tf_odom_state_publisher_ = + std::make_unique(tf_odom_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + rt_tf_odom_state_publisher_->lock(); + rt_tf_odom_state_publisher_->msg_.transforms.resize(1); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; + rt_tf_odom_state_publisher_->unlock(); + + try + { + // State publisher + controller_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = + std::make_unique(controller_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + controller_state_publisher_->lock(); + controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->unlock(); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +void SteeringControllersLibrary::reference_callback( + const std::shared_ptr msg) +{ + // if no timestamp provided use current time for command timestamp + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + input_ref_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +void SteeringControllersLibrary::reference_callback_unstamped( + const std::shared_ptr msg) +{ + RCLCPP_WARN( + get_node()->get_logger(), + "Use of Twist message without stamped is deprecated and it will be removed in ROS 2 J-Turtle " + "version. Use '~/reference' topic with 'geometry_msgs::msg::TwistStamped' message type in the " + "future."); + auto twist_stamped = *(input_ref_.readFromNonRT()); + twist_stamped->header.stamp = get_node()->now(); + // if no timestamp provided use current time for command timestamp + if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + twist_stamped->header.stamp = get_node()->now(); + } + + const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + twist_stamped->twist = *msg; + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + +controller_interface::InterfaceConfiguration +SteeringControllersLibrary::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.reserve(nr_cmd_itfs_); + + if (params_.front_steering) + { + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + else + { + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +SteeringControllersLibrary::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(nr_state_itfs_); + const auto traction_wheels_feedback = params_.position_feedback + ? hardware_interface::HW_IF_POSITION + : hardware_interface::HW_IF_VELOCITY; + if (params_.front_steering) + { + for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + rear_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } + + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + front_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + else + { + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + front_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } + + for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + rear_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + + return state_interfaces_config; +} + +std::vector +SteeringControllersLibrary::on_export_reference_interfaces() +{ + reference_interfaces_.resize(nr_ref_itfs_, std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(nr_ref_itfs_); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, + &reference_interfaces_[0])); + + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_POSITION, + &reference_interfaces_[1])); + + return reference_interfaces; +} + +bool SteeringControllersLibrary::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for (size_t i = 0; i < nr_cmd_itfs_; ++i) + { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( + 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)) + { + 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(); + } + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type SteeringControllersLibrary::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + update_odometry(period); + + // MOVE ROBOT + + // Limit velocities and accelerations: + // TODO(destogl): add limiter for the velocities + + if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) + { + // store and set commands + const double linear_command = reference_interfaces_[0]; + const double angular_command = reference_interfaces_[1]; + auto [traction_commands, steering_commands] = + odometry_.get_commands(linear_command, angular_command); + if (params_.front_steering) + { + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(traction_commands[i]); + } + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]); + } + } + else + { + { + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(traction_commands[i]); + } + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i + params_.front_wheels_names.size()].set_value( + steering_commands[i]); + } + } + } + } + + // Publish odometry message + // Compute and store orientation info + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.get_heading()); + + // Populate odom message and publish + if (rt_odom_state_publisher_->trylock()) + { + rt_odom_state_publisher_->msg_.header.stamp = time; + rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.get_x(); + rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.get_y(); + rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); + rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.get_linear(); + rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.get_angular(); + rt_odom_state_publisher_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + { + rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = + odometry_.get_x(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = + odometry_.get_y(); + rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = + tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->unlockAndPublish(); + } + + if (controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = time; + controller_state_publisher_->msg_.traction_wheels_position.clear(); + controller_state_publisher_->msg_.traction_wheels_velocity.clear(); + controller_state_publisher_->msg_.linear_velocity_command.clear(); + controller_state_publisher_->msg_.steer_positions.clear(); + controller_state_publisher_->msg_.steering_angle_command.clear(); + + auto number_of_traction_wheels = params_.rear_wheels_names.size(); + auto number_of_steering_wheels = params_.front_wheels_names.size(); + + if (!params_.front_steering) + { + number_of_traction_wheels = params_.front_wheels_names.size(); + number_of_steering_wheels = params_.rear_wheels_names.size(); + } + + for (size_t i = 0; i < number_of_traction_wheels; ++i) + { + if (params_.position_feedback) + { + controller_state_publisher_->msg_.traction_wheels_position.push_back( + state_interfaces_[i].get_value()); + } + else + { + controller_state_publisher_->msg_.traction_wheels_velocity.push_back( + state_interfaces_[i].get_value()); + } + controller_state_publisher_->msg_.linear_velocity_command.push_back( + command_interfaces_[i].get_value()); + } + + for (size_t i = 0; i < number_of_steering_wheels; ++i) + { + controller_state_publisher_->msg_.steer_positions.push_back( + state_interfaces_[number_of_traction_wheels + i].get_value()); + controller_state_publisher_->msg_.steering_angle_command.push_back( + command_interfaces_[number_of_traction_wheels + i].get_value()); + } + + controller_state_publisher_->unlockAndPublish(); + } + + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + + return controller_interface::return_type::OK; +} + +} // namespace steering_controllers_library diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml new file mode 100644 index 0000000000..86acb01dae --- /dev/null +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -0,0 +1,122 @@ +steering_controllers_library: + reference_timeout: { + type: double, + default_value: 1, + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", + } + + front_steering: { + type: bool, + default_value: true, + description: "Is the steering on the front of the robot?", + read_only: true, + } + + rear_wheels_names: { + type: string_array, + description: "Names of rear wheel joints.", + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + not_empty<>: null, + } + } + + front_wheels_names: { + type: string_array, + description: "Names of front wheel joints.", + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + not_empty<>: null, + } + } + + rear_wheels_state_names: { + type: string_array, + description: "(Optional) Names of rear wheel joints to read states from. If not set joint names from 'rear_wheels_names' will be used.", + default_value: [], + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + } + } + + front_wheels_state_names: { + type: string_array, + description: "(Optional) Names of front wheel joints to read states from. If not set joint names from 'front_wheels_names' will be used.", + default_value: [], + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + } + } + + open_loop: { + type: bool, + default_value: false, + description: "bool parameter decides if open oop or not (feedback).", + read_only: false, + } + + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "The number of velocity samples to average together to compute the odometry twist.linear.x and twist.angular.z velocities.", + read_only: false, + } + + base_frame_id: { + type: string, + default_value: "base_link", + description: "Base frame_id set to value of base_frame_id.", + read_only: false, + } + + odom_frame_id: { + type: string, + default_value: "odom", + description: "Odometry frame_id set to value of odom_frame_id.", + read_only: false, + } + + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publishing to tf is enabled or disabled ?.", + read_only: false, + } + + twist_covariance_diagonal: { + type: double_array, + default_value: [0, 7, 14, 21, 28, 35], + description: "diagonal values of twist covariance matrix.", + read_only: false, + } + + pose_covariance_diagonal: { + type: double_array, + default_value: [0, 7, 14, 21, 28, 35], + description: "diagonal values of pose covariance matrix.", + read_only: false, + } + + position_feedback: { + type: bool, + default_value: false, + description: "Choice of feedback type, if position_feedback is false then HW_IF_VELOCITY is taken as interface type, if + position_feedback is true then HW_IF_POSITION is taken as interface type", + read_only: false, + } + + use_stamped_vel: { + type: bool, + default_value: false, + description: "Choice of vel type, if use_stamped_vel is false then geometry_msgs::msg::Twist is taken as vel msg type, if + use_stamped_vel is true then geometry_msgs::msg::TwistStamped is taken as vel msg type", + read_only: false, + } diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp new file mode 100644 index 0000000000..c480dc1253 --- /dev/null +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -0,0 +1,333 @@ +/********************************************************************* + * Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *********************************************************************/ + +/* + * Author: dr. sc. Tomislav Petkovic + * Author: Dr. Ing. Denis Stogl + */ + +#include "steering_controllers_library/steering_odometry.hpp" + +#include +#include + +namespace steering_odometry +{ +SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) +: timestamp_(0.0), + x_(0.0), + y_(0.0), + heading_(0.0), + linear_(0.0), + angular_(0.0), + wheel_track_(0.0), + wheelbase_(0.0), + wheel_radius_(0.0), + traction_wheel_old_pos_(0.0), + velocity_rolling_window_size_(velocity_rolling_window_size), + linear_acc_(velocity_rolling_window_size), + angular_acc_(velocity_rolling_window_size) +{ +} + +void SteeringOdometry::init(const rclcpp::Time & time) +{ + // Reset accumulators and timestamp: + reset_accumulators(); + timestamp_ = time; +} + +bool SteeringOdometry::update_odometry( + const double linear_velocity, const double angular, const double dt) +{ + /// Integrate odometry: + SteeringOdometry::integrate_exact(linear_velocity * dt, angular); + + /// We cannot estimate the speed with very small time intervals: + if (dt < 0.0001) + { + return false; // Interval too small to integrate with + } + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_.accumulate(linear_velocity); + angular_acc_.accumulate(angular / dt); + + linear_ = linear_acc_.getRollingMean(); + angular_ = angular_acc_.getRollingMean(); + + return true; +} + +bool SteeringOdometry::update_from_position( + const double traction_wheel_pos, const double steer_pos, const double dt) +{ + /// 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_; + + /// Update old position with current: + traction_wheel_old_pos_ = traction_wheel_cur_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); +} + +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_; + const double traction_left_wheel_est_pos_diff = + traction_left_wheel_cur_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; + + 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); +} + +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_; + const double traction_left_wheel_est_pos_diff = + traction_left_wheel_cur_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; + + /// 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); +} + +bool SteeringOdometry::update_from_velocity( + const double traction_wheel_vel, const double steer_pos, const double dt) +{ + steer_pos_ = steer_pos; + double linear_velocity = traction_wheel_vel * wheel_radius_; + const double angular = tan(steer_pos) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular, dt); +} + +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; + + const double angular = tan(steer_pos_) * linear_velocity / wheelbase_; + + return update_odometry(linear_velocity, angular, 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); +} + +void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + SteeringOdometry::integrate_exact(linear * dt, angular * dt); +} + +void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track) +{ + wheel_radius_ = wheel_radius; + wheelbase_ = wheelbase; + wheel_track_ = wheel_track; +} + +void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; + + reset_accumulators(); +} + +void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } + +double SteeringOdometry::convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot) +{ + if (theta_dot == 0 || Vx == 0) + { + return 0; + } + return std::atan(theta_dot * wheelbase_ / Vx); +} + +std::tuple, std::vector> SteeringOdometry::get_commands( + double Vx, double theta_dot) +{ + // desired velocity and steering angle of the middle of traction and steering axis + double Ws, alpha; + + if (Vx == 0 && theta_dot != 0) + { + alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(theta_dot) * wheelbase_ / wheel_radius_; + } + else + { + alpha = SteeringOdometry::convert_trans_rot_vel_to_steering_angle(Vx, theta_dot); + Ws = Vx / (wheel_radius_ * std::cos(steer_pos_)); + } + + if (config_type_ == BICYCLE_CONFIG) + { + std::vector traction_commands = {Ws}; + std::vector steering_commands = {alpha}; + 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) + { + 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; + traction_commands = {Wr, Wl}; + } + steering_commands = {alpha}; + 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) + { + traction_commands = {Ws, Ws}; + steering_commands = {alpha, alpha}; + } + 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; + 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); + + double alpha_r = std::atan2(numerator, denominator_first_member - denominator_second_member); + double alpha_l = std::atan2(numerator, denominator_first_member + denominator_second_member); + steering_commands = {alpha_r, alpha_l}; + } + return std::make_tuple(traction_commands, steering_commands); + } + else + { + throw std::runtime_error("Config not implemented"); + } +} + +void SteeringOdometry::reset_odometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; + reset_accumulators(); +} + +void SteeringOdometry::integrate_runge_kutta_2(double linear, double angular) +{ + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; +} + +/** + * \brief Other possible integration method provided by the class + * \param linear + * \param angular + */ +void SteeringOdometry::integrate_exact(double linear, double angular) +{ + if (fabs(angular) < 1e-6) + { + integrate_runge_kutta_2(linear, angular); + } + else + { + /// Exact integration (should solve problems when angular is zero): + 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)); + } +} + +void SteeringOdometry::reset_accumulators() +{ + linear_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); + angular_acc_ = rcpputils::RollingMeanAccumulator(velocity_rolling_window_size_); +} + +} // namespace steering_odometry diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml new file mode 100644 index 0000000000..d200d34961 --- /dev/null +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -0,0 +1,17 @@ +test_steering_controllers_library: + ros__parameters: + + reference_timeout: 0.1 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [front_right_steering_joint, front_left_steering_joint] + + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp new file mode 100644 index 0000000000..81075d1082 --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -0,0 +1,200 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_steering_controllers_library.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +class SteeringControllersLibraryTest +: public SteeringControllersLibraryFixture +{ +}; + +// checking if all interfaces, command, state and reference are exported as expected +TEST_F(SteeringControllersLibraryTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_LEFT_WHEEL], + front_wheels_names_[1] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_LEFT_WHEEL], + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + + // check ref itfsTIME + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +// Tests controller update_reference_from_subscribers and +// its two cases for position_feedback true/false behavior +// when too old msg is sent i.e age_of_last_command > ref_timeout case +TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + const double TEST_LINEAR_VELOCITY_X = 1.5; + const double TEST_LINEAR_VELOCITY_Y = 0.0; + const double TEST_ANGULAR_VELOCITY_Z = 0.3; + + std::shared_ptr msg = std::make_shared(); + + // adjusting to achieve age_of_last_command > ref_timeout + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + + // case 1 position_feedback = false + controller_->params_.position_feedback = false; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); + } + + // case 2 position_feedback = true + controller_->params_.position_feedback = true; + + // adjusting to achieve age_of_last_command > ref_timeout + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->twist.linear.x = TEST_LINEAR_VELOCITY_X; + msg->twist.linear.y = TEST_LINEAR_VELOCITY_Y; + msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = std::numeric_limits::quiet_NaN(); + msg->twist.angular.y = std::numeric_limits::quiet_NaN(); + msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; + controller_->input_ref_.writeFromNonRT(msg); + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) + { + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp new file mode 100644 index 0000000000..c72c61257a --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -0,0 +1,341 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ +#define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// NOTE: Testing steering_controllers_library for ackermann vehicle configuration only + +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; +static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2; +static constexpr size_t CMD_STEER_LEFT_WHEEL = 3; + +static constexpr size_t NR_STATE_ITFS = 4; +static constexpr size_t NR_CMD_ITFS = 4; +static constexpr size_t NR_REF_ITFS = 2; + +static constexpr double WHEELBASE_ = 3.24644; +static constexpr double FRONT_WHEEL_TRACK_ = 2.12321; +static constexpr double REAR_WHEEL_TRACK_ = 1.76868; +static constexpr double FRONT_WHEELS_RADIUS_ = 0.45; +static constexpr double REAR_WHEELS_RADIUS_ = 0.45; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableSteeringControllersLibrary +: public steering_controllers_library::SteeringControllersLibrary +{ + FRIEND_TEST(SteeringControllersLibraryTest, check_exported_intefaces); + FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); + +public: + 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; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return steering_controllers_library::SteeringControllersLibrary::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerReferenceMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool 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); + } + + // implementing methods which are declared virtual in the steering_controllers_library.hpp + void initialize_implementation_parameter_listener() + { + param_listener_ = std::make_shared(get_node()); + } + + controller_interface::CallbackReturn configure_odometry() + { + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + odometry_.set_wheel_params(FRONT_WHEELS_RADIUS_, WHEELBASE_, REAR_WHEEL_TRACK_); + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + return controller_interface::CallbackReturn::SUCCESS; + } + + bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class SteeringControllersLibraryFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_steering_controllers_library/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_steering_controllers_library") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_steering_controllers_library/controller_state", 10, subs_callback); + + // 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)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + 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 + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = { + "front_right_steering_joint", "front_left_steering_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; + + double wheelbase_ = 3.24644; + double front_wheel_track_ = 2.12321; + double rear_wheel_track_ = 1.76868; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; + + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index bf0c5a0102..f58f8f1539 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- +* enable ReflowComments to also use ColumnLimit on comments (`#625 `_) +* Contributors: Sai Kishor Kothakota + +3.9.0 (2023-05-28) +------------------ +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-05-14) +------------------ + 3.7.0 (2023-05-02) ------------------ diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index ea2232f88f..291f08ad9f 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(tricycle_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst index a248bbec96..d153aeacba 100644 --- a/tricycle_controller/doc/userdoc.rst +++ b/tricycle_controller/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/tricycle_controller/doc/userdoc.rst + .. _tricycle_controller_userdoc: tricycle_controller diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 83cb39cc7f..2b5106ff83 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 3.7.0 + 3.11.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp index b9759e3624..a8019714e1 100644 --- a/tricycle_controller/src/traction_limiter.cpp +++ b/tricycle_controller/src/traction_limiter.cpp @@ -102,7 +102,7 @@ double TractionLimiter::limit_acceleration(double & v, double v0, double dt) double dv_min; double dv_max; - if (abs(v) >= abs(v0)) + if (std::fabs(v) >= std::fabs(v0)) { dv_min = min_acceleration_ * dt; dv_max = max_acceleration_ * dt; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 4f4e22ec9d..d6aeec0af1 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -67,7 +67,7 @@ CallbackReturn TricycleController::on_init() auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); auto_declare("odom_only_twist", odom_params_.odom_only_twist); - auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count()); + auto_declare("cmd_vel_timeout", static_cast(cmd_vel_timeout_.count())); auto_declare("publish_ackermann_command", publish_ackermann_command_); auto_declare("velocity_rolling_window_size", 10); auto_declare("use_stamped_vel", use_stamped_vel_); @@ -234,8 +234,8 @@ controller_interface::return_type TricycleController::update( AckermannDrive ackermann_command; // speed in AckermannDrive is defined as desired forward speed (m/s) but it is used here as wheel // speed (rad/s) - ackermann_command.speed = Ws_write; - ackermann_command.steering_angle = alpha_write; + ackermann_command.speed = static_cast(Ws_write); + ackermann_command.steering_angle = static_cast(alpha_write); previous_commands_.emplace(ackermann_command); // Publish ackermann command @@ -244,8 +244,8 @@ controller_interface::return_type TricycleController::update( auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel // speed (rad/s) - realtime_ackermann_command.speed = Ws_write; - realtime_ackermann_command.steering_angle = alpha_write; + realtime_ackermann_command.speed = static_cast(Ws_write); + realtime_ackermann_command.steering_angle = static_cast(alpha_write); realtime_ackermann_command_publisher_->unlockAndPublish(); } diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index a1f09ddaf8..1d6edf261a 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -52,12 +52,12 @@ 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 - */ + * @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( rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst new file mode 100644 index 0000000000..3782216e48 --- /dev/null +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -0,0 +1,141 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tricycle_steering_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Fix sphinx for steering odometry library/controllers (`#626 `_) +* Steering odometry library and controllers (`#484 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković + +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/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..4c56d68185 --- /dev/null +++ b/tricycle_steering_controller/CMakeLists.txt @@ -0,0 +1,105 @@ +cmake_minimum_required(VERSION 3.16) +project(tricycle_steering_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(tricycle_steering_controller_parameters + src/tricycle_steering_controller.yaml +) + +add_library( + tricycle_steering_controller + SHARED + src/tricycle_steering_controller.cpp +) +target_compile_features(tricycle_steering_controller PUBLIC cxx_std_17) +target_include_directories(tricycle_steering_controller PUBLIC + "$" + "$") +target_link_libraries(tricycle_steering_controller PUBLIC + tricycle_steering_controller_parameters) +ament_target_dependencies(tricycle_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(tricycle_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface tricycle_steering_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock(test_load_tricycle_steering_controller + test/test_load_tricycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml + ) + ament_target_dependencies(test_load_tricycle_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_tricycle_steering_controller test/test_tricycle_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml) + target_include_directories(test_tricycle_steering_controller PRIVATE include) + target_link_libraries(test_tricycle_steering_controller tricycle_steering_controller) + ament_target_dependencies( + test_tricycle_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_tricycle_steering_controller_preceeding test/test_tricycle_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_preceeding_params.yaml) + target_include_directories(test_tricycle_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_tricycle_steering_controller_preceeding tricycle_steering_controller) + ament_target_dependencies( + test_tricycle_steering_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/tricycle_steering_controller +) + +install( + TARGETS tricycle_steering_controller tricycle_steering_controller_parameters + EXPORT export_tricycle_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_tricycle_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/tricycle_steering_controller/doc/userdoc.rst b/tricycle_steering_controller/doc/userdoc.rst new file mode 100644 index 0000000000..d76f489745 --- /dev/null +++ b/tricycle_steering_controller/doc/userdoc.rst @@ -0,0 +1,22 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/tricycle_steering_controller/doc/userdoc.rst + +.. _tricycle_steering_controller_userdoc: + +tricycle_steering_controller +============================= + +This controller implements the kinematics with two axes and three wheels, where two wheels on an axis are fixed (traction/drive), and the wheel on the other axis is steerable. + +The controller expects to have two commanding joints for traction, one for each fixed wheel and one commanding joint for steering. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +For an exemplary parameterization see the ``test`` folder of the controller's package. + +.. generate_parameter_library_details:: ../src/tricycle_steering_controller.yaml diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp new file mode 100644 index 0000000000..607a684df5 --- /dev/null +++ b/tricycle_steering_controller/include/tricycle_steering_controller/tricycle_steering_controller.hpp @@ -0,0 +1,63 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ +#define TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ + +#include + +#include "steering_controllers_library/steering_controllers_library.hpp" +#include "tricycle_steering_controller/visibility_control.h" +#include "tricycle_steering_controller_parameters.hpp" + +namespace tricycle_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_AXIS = 2; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_WHEEL = 2; + +static constexpr size_t NR_STATE_ITFS = 3; +static constexpr size_t NR_CMD_ITFS = 3; +static constexpr size_t NR_REF_ITFS = 2; + +class TricycleSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + TricycleSteeringController(); + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC controller_interface::CallbackReturn configure_odometry() + override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + STEERING_CONTROLLERS__VISIBILITY_PUBLIC void initialize_implementation_parameter_listener() + override; + +protected: + std::shared_ptr tricycle_param_listener_; + tricycle_steering_controller::Params tricycle_params_; +}; +} // namespace tricycle_steering_controller + +#endif // TRICYCLE_STEERING_CONTROLLER__TRICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h new file mode 100644 index 0000000000..606b067ad8 --- /dev/null +++ b/tricycle_steering_controller/include/tricycle_steering_controller/visibility_control.h @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TRICYCLE_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define TRICYCLE_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // TRICYCLE_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml new file mode 100644 index 0000000000..b8893108e0 --- /dev/null +++ b/tricycle_steering_controller/package.xml @@ -0,0 +1,38 @@ + + + + tricycle_steering_controller + 3.11.0 + Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. + Apache License 2.0 + Bence Magyar + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + Tony Najjar + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp new file mode 100644 index 0000000000..f89d78a52c --- /dev/null +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -0,0 +1,93 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tricycle_steering_controller/tricycle_steering_controller.hpp" + +namespace tricycle_steering_controller +{ +TricycleSteeringController::TricycleSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} +void TricycleSteeringController::initialize_implementation_parameter_listener() +{ + tricycle_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn TricycleSteeringController::configure_odometry() +{ + tricycle_params_ = tricycle_param_listener_->get_params(); + + const double front_wheels_radius = tricycle_params_.front_wheels_radius; + const double rear_wheels_radius = tricycle_params_.rear_wheels_radius; + const double wheel_track = tricycle_params_.wheel_track; + const double wheelbase = tricycle_params_.wheelbase; + + if (params_.front_steering) + { + odometry_.set_wheel_params(rear_wheels_radius, wheelbase, wheel_track); + } + else + { + odometry_.set_wheel_params(front_wheels_radius, wheelbase, wheel_track); + } + + odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + + RCLCPP_INFO(get_node()->get_logger(), "tricycle odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + const double 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(); + if ( + !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && + !std::isnan(steer_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()); + } + 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()); + } + } + } + return true; +} + +} // namespace tricycle_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + tricycle_steering_controller::TricycleSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml new file mode 100644 index 0000000000..1015865fd9 --- /dev/null +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -0,0 +1,32 @@ +tricycle_steering_controller: + wheel_track: + { + type: double, + default_value: 0.0, + description: "Wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + front_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Front wheels radius.", + read_only: false, + } + + rear_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Rear wheels radius.", + read_only: false, + } diff --git a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp new file mode 100644 index 0000000000..0d04afdf38 --- /dev/null +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadTricycleSteeringController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_tricycle_steering_controller", + "tricycle_steering_controller/TricycleSteeringController"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp new file mode 100644 index 0000000000..82ba924305 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -0,0 +1,274 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_tricycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class TricycleSteeringControllerTest +: public TricycleSteeringControllerFixture +{ +}; + +TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); +} + +TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + + // check ref itfsTIME + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +TEST_F(TricycleSteeringControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(TricycleSteeringControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(TricycleSteeringControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(TricycleSteeringControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(TricycleSteeringControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.steering_angle_command[0], 2.2); + + publish_commands(); + ASSERT_TRUE(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_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + subscribe_and_get_messages(msg); + + EXPECT_NEAR( + msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + msg.linear_velocity_command[STATE_TRACTION_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp new file mode 100644 index 0000000000..422f399ad4 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -0,0 +1,302 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ +#define TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "tricycle_steering_controller/tricycle_steering_controller.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using tricycle_steering_controller::STATE_STEER_AXIS; +using tricycle_steering_controller::STATE_TRACTION_LEFT_WHEEL; +using tricycle_steering_controller::STATE_TRACTION_RIGHT_WHEEL; + +// name constants for command interfaces +using tricycle_steering_controller::CMD_STEER_WHEEL; +using tricycle_steering_controller::CMD_TRACTION_LEFT_WHEEL; +using tricycle_steering_controller::CMD_TRACTION_RIGHT_WHEEL; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableTricycleSteeringController +: public tricycle_steering_controller::TricycleSteeringController +{ + FRIEND_TEST(TricycleSteeringControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(TricycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(TricycleSteeringControllerTest, activate_success); + FRIEND_TEST(TricycleSteeringControllerTest, update_success); + FRIEND_TEST(TricycleSteeringControllerTest, deactivate_success); + FRIEND_TEST(TricycleSteeringControllerTest, reactivate_success); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic); + FRIEND_TEST(TricycleSteeringControllerTest, test_update_logic_chained); + FRIEND_TEST(TricycleSteeringControllerTest, publish_status_success); + FRIEND_TEST(TricycleSteeringControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + 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; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return tricycle_steering_controller::TricycleSteeringController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerReferenceMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool 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); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class TricycleSteeringControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_tricycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_tricycle_steering_controller/controller_state", 10, subs_callback); + + // 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); + // 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)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + 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 + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + bool use_stamped_vel_ = true; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = {"steering_axis_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; + + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0]}; + + double wheelbase_ = 3.24644; + double wheel_track_ = 1.212121; + + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_TRICYCLE_STEERING_CONTROLLER_HPP_ diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp new file mode 100644 index 0000000000..dd72332875 --- /dev/null +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -0,0 +1,100 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_tricycle_steering_controller.hpp" + +#include +#include +#include +#include +#include + +class TricycleSteeringControllerTest +: public TricycleSteeringControllerFixture +{ +}; + +TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.rear_wheels_names, + testing::ElementsAreArray(rear_wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.front_wheels_names, + testing::ElementsAreArray(front_wheels_preceeding_names_)); + ASSERT_EQ(controller_->params_.front_steering, front_steering_); + ASSERT_EQ(controller_->params_.open_loop, open_loop_); + ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); + ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); + ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); +} + +TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + command_intefaces.names[CMD_STEER_WHEEL], + preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_intefaces.names[STATE_STEER_AXIS], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml new file mode 100644 index 0000000000..6bfb87a892 --- /dev/null +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -0,0 +1,16 @@ +test_tricycle_steering_controller: + ros__parameters: + + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [steering_axis_joint] + + wheelbase: 3.24644 + wheel_track: 1.212121 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml new file mode 100644 index 0000000000..ea8b88002e --- /dev/null +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -0,0 +1,16 @@ +test_tricycle_steering_controller: + ros__parameters: + reference_timeout: 2.0 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + front_wheels_names: [pid_controller/steering_axis_joint] + rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_state_names: [steering_axis_joint] + wheelbase: 3.24644 + wheel_track: 1.212121 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/tricycle_steering_controller/tricycle_steering_controller.xml b/tricycle_steering_controller/tricycle_steering_controller.xml new file mode 100644 index 0000000000..f0d5d85467 --- /dev/null +++ b/tricycle_steering_controller/tricycle_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Tricycle kinematics with two traction and one steering joint. + + + diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 4d584c209f..b86820e783 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Contributors: gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Use branch name substitution for all links (`#618 `_) +* Fix github links on control.ros.org (`#604 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-05-14) +------------------ + 3.7.0 (2023-05-02) ------------------ diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 27ec8417bb..3642ae1cb9 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(velocity_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/velocity_controllers/doc/userdoc.rst b/velocity_controllers/doc/userdoc.rst index 62b6aa019f..fa7e36062e 100644 --- a/velocity_controllers/doc/userdoc.rst +++ b/velocity_controllers/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/velocity_controllers/doc/userdoc.rst + .. _velocity_controllers_userdoc: velocity_controllers diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 4bbf7a2723..eba1e97ad2 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 3.7.0 + 3.11.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios