From af340a5eb6b07b0711e81e419fe9bb183391b7fd Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 11 Jan 2023 19:45:11 +0100 Subject: [PATCH 01/21] =?UTF-8?q?=F0=9F=94=A7=20Fixes=20and=20updated=20on?= =?UTF-8?q?=20pre-commit=20hooks=20and=20their=20action=20(#492)=20(#496)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit (cherry picked from commit 3829d33f236932890548b25df2812c3eb5a589a6) Co-authored-by: Denis Å togl --- .github/workflows/ci-format.yml | 8 +++---- .pre-commit-config.yaml | 22 +++++++++---------- .../resource/joint_trajectory_controller.ui | 2 +- .../update_combo.py | 3 ++- 4 files changed, 18 insertions(+), 17 deletions(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index dd78cca0ed..b54c76d3e5 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,11 +12,11 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v2 + - uses: actions/setup-python@v4.4.0 with: - python-version: 3.9 + python-version: '3.10' - name: Install system hooks - run: sudo apt install -qq clang-format-12 cppcheck - - uses: pre-commit/action@v2.0.3 + run: sudo apt install -qq clang-format-14 cppcheck + - uses: pre-commit/action@v3.0.0 with: extra_args: --all-files --hook-stage manual diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 58fe46dd90..b927aeac78 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.3.0 + rev: v4.4.0 hooks: - id: check-added-large-files - id: check-ast @@ -33,26 +33,26 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.37.3 + rev: v3.3.1 hooks: - id: pyupgrade args: [--py36-plus] - repo: https://github.com/psf/black - rev: 22.6.0 + rev: 22.12.0 hooks: - id: black args: ["--line-length=99"] - # PEP 257 - - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 - rev: v0.3.3 + # PyDocStyle + - repo: https://github.com/PyCQA/pydocstyle + rev: 6.2.2 hooks: - - id: pep257 + - id: pydocstyle args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 - rev: 5.0.4 + rev: 6.0.0 hooks: - id: flake8 args: ["--ignore=E501"] @@ -63,7 +63,7 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-12 + entry: clang-format-14 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] @@ -119,7 +119,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.0.0 + rev: v1.1.1 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -136,7 +136,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.1.0 + rev: v2.2.2 hooks: - id: codespell args: ['--write-changes', '--uri-ignore-words-list=ist'] diff --git a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui index 3f3b6c1186..bc43f202e8 100644 --- a/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui +++ b/rqt_joint_trajectory_controller/resource/joint_trajectory_controller.ui @@ -31,7 +31,7 @@ - <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namesapce.</p></body></html> + <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namespace.</p></body></html> controller manager ns diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py index 4702d7fbc5..f0b632322f 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -48,7 +48,8 @@ def update_combo(combo, new_vals): def _is_permutation(a, b): - """Check is arrays are permutation of each other. + """ + Check is arrays are permutation of each other. @type a [] first array with values to compare with the second one. @type b [] second array with values to compare with the first one. From 7e98aa5383682845263ae6bf65ae1e812ec56d33 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 11 Jan 2023 22:30:32 +0100 Subject: [PATCH 02/21] [CI] Execute workflows on humble branch (#497) --- .github/workflows/humble-abi-compatibility.yml | 4 ++-- .github/workflows/humble-binary-build-main.yml | 8 ++++---- .github/workflows/humble-binary-build-testing.yml | 8 ++++---- .github/workflows/humble-rhel-binary-build.yml | 8 ++++---- .github/workflows/humble-semi-binary-build-main.yml | 10 +++++----- .github/workflows/humble-semi-binary-build-testing.yml | 10 +++++----- .github/workflows/humble-source-build.yml | 6 +++--- ros2_controllers.humble.repos | 2 +- 8 files changed, 28 insertions(+), 28 deletions(-) diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 1be00cfc76..3f38d5b6ce 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -2,10 +2,10 @@ name: Humble - ABI Compatibility Check on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble jobs: abi_check: diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/humble-binary-build-main.yml index 01708cf864..791dbdb4a5 100644 --- a/.github/workflows/humble-binary-build-main.yml +++ b/.github/workflows/humble-binary-build-main.yml @@ -5,16 +5,16 @@ name: Humble Binary Build - main on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 2 * * *' jobs: binary: diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml index 73ed0a4859..6b4cf43e30 100644 --- a/.github/workflows/humble-binary-build-testing.yml +++ b/.github/workflows/humble-binary-build-testing.yml @@ -5,16 +5,16 @@ name: Humble Binary Build - testing on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 2 * * *' jobs: binary: diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 8f4a65a9b5..e63d047a72 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -2,16 +2,16 @@ name: Humble RHEL Binary Build on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 1 * * *' + - cron: '03 2 * * *' jobs: diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml index 10b1186b79..41c7ebdffc 100644 --- a/.github/workflows/humble-semi-binary-build-main.yml +++ b/.github/workflows/humble-semi-binary-build-main.yml @@ -4,16 +4,16 @@ name: Humble Semi-Binary Build - main on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' + - cron: '33 2 * * *' jobs: semi_binary: @@ -22,4 +22,4 @@ jobs: ros_distro: humble ros_repo: main upstream_workspace: ros2_controllers.humble.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-semi-binary-build-testing.yml b/.github/workflows/humble-semi-binary-build-testing.yml index ec73cc6b98..ef290ce065 100644 --- a/.github/workflows/humble-semi-binary-build-testing.yml +++ b/.github/workflows/humble-semi-binary-build-testing.yml @@ -4,16 +4,16 @@ name: Humble Semi-Binary Build - testing on: workflow_dispatch: branches: - - master + - humble pull_request: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' + - cron: '33 2 * * *' jobs: semi_binary: @@ -22,4 +22,4 @@ jobs: ros_distro: humble ros_repo: testing upstream_workspace: ros2_controllers.humble.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: humble diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 18c33b6d52..2455e20d4a 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -2,13 +2,13 @@ name: Humble Source Build on: workflow_dispatch: branches: - - master + - humble push: branches: - - master + - humble schedule: # Run every day to detect flakiness and broken dependencies - - cron: '03 3 * * *' + - cron: '03 4 * * *' jobs: source: diff --git a/ros2_controllers.humble.repos b/ros2_controllers.humble.repos index 8c0ba1ff8a..2a3b96551d 100644 --- a/ros2_controllers.humble.repos +++ b/ros2_controllers.humble.repos @@ -2,7 +2,7 @@ repositories: ros2_control: type: git url: https://github.com/ros-controls/ros2_control.git - version: master + version: humble realtime_tools: type: git url: https://github.com/ros-controls/realtime_tools.git From 30c6a5cdfd9bc783136e994c9faa7bf8603d1cf5 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 11 Jan 2023 23:39:48 +0100 Subject: [PATCH 03/21] Add backward_ros to all controllers (#489) (#493) (cherry picked from commit f6c4769eae0389cad62596291f88ec041558f02c) Co-authored-by: Bence Magyar --- admittance_controller/CMakeLists.txt | 1 + admittance_controller/package.xml | 1 + diff_drive_controller/CMakeLists.txt | 1 + diff_drive_controller/package.xml | 1 + effort_controllers/CMakeLists.txt | 1 + effort_controllers/package.xml | 1 + force_torque_sensor_broadcaster/CMakeLists.txt | 1 + force_torque_sensor_broadcaster/package.xml | 1 + forward_command_controller/CMakeLists.txt | 1 + forward_command_controller/package.xml | 1 + gripper_controllers/CMakeLists.txt | 1 + gripper_controllers/package.xml | 1 + imu_sensor_broadcaster/CMakeLists.txt | 1 + imu_sensor_broadcaster/package.xml | 1 + joint_state_broadcaster/CMakeLists.txt | 1 + joint_state_broadcaster/package.xml | 1 + joint_trajectory_controller/CMakeLists.txt | 1 + joint_trajectory_controller/package.xml | 1 + position_controllers/CMakeLists.txt | 1 + position_controllers/package.xml | 1 + tricycle_controller/CMakeLists.txt | 1 + tricycle_controller/package.xml | 1 + velocity_controllers/CMakeLists.txt | 1 + velocity_controllers/package.xml | 1 + 24 files changed, 24 insertions(+) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index a6dcf35dce..3f121d9c3c 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -34,6 +34,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 876ed424b0..69180a290f 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros control_msgs control_toolbox controller_interface diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 83f18a9931..7188f09f18 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -26,6 +26,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 8b2e368824..4feec7fe37 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake generate_parameter_library + backward_ros controller_interface geometry_msgs hardware_interface diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index 91632606bf..d0e92dda9a 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 7476889b73..0ca0d8dffb 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros forward_command_controller rclcpp diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 3aa57d4aad..eac3fe9d8b 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 8cee3ef6a3..174499c573 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros controller_interface geometry_msgs hardware_interface diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index ef8581d195..5dacdf66c1 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 7853d78542..f6ded4ff93 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros controller_interface generate_parameter_library hardware_interface diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 97ccd2788d..e1b32ac2b3 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -29,6 +29,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index b033e99b12..2734c54933 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -14,6 +14,7 @@ ament_cmake + backward_ros control_msgs control_toolbox controller_interface diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index e49fb65ad3..ef9d5e14e4 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 79eb5e14a7..40836967c3 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros controller_interface generate_parameter_library hardware_interface diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 0717577e15..ee46d8eaab 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -12,6 +12,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(control_msgs REQUIRED) find_package(controller_interface REQUIRED) find_package(generate_parameter_library REQUIRED) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index c24d274a93..9fd5942d5d 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -18,6 +18,7 @@ pluginlib rcutils + backward_ros control_msgs controller_interface generate_parameter_library diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 32461617ac..9cc58b7ef8 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -26,6 +26,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 3e806ce59e..a589768776 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -19,6 +19,7 @@ pluginlib realtime_tools + backward_ros controller_interface control_msgs control_toolbox diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index f1f57c6180..faf32a0e0b 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 21cdd59b86..086afccc1f 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros forward_command_controller rclcpp diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index c499bca983..e9342e8696 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -7,6 +7,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(controller_interface REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 1681367c25..3c853872e9 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -11,6 +11,7 @@ ament_cmake + backward_ros controller_interface geometry_msgs hardware_interface diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 5f0560659a..4d7ad07dbd 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 131bcd11f0..03f1bed657 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -10,6 +10,7 @@ ament_cmake + backward_ros forward_command_controller rclcpp From aeffc9f44998124cda362530db3bce5e8ceb2d2f Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 12 Jan 2023 00:09:00 +0100 Subject: [PATCH 04/21] diff_drive base_frame_id param (#495) (#498) (cherry picked from commit acaf91837838635fc1cde8da1a23498a14f03c12) Co-authored-by: Jakub Delicat <109142865+delihus@users.noreply.github.com> --- diff_drive_controller/src/diff_drive_controller_parameter.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 82ba210113..fb50f2fb50 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -46,7 +46,7 @@ diff_drive_controller: } base_frame_id: { type: string, - default_value: "odom", + default_value: "base_link", description: "Name of the robot's base frame that is child of the odometry frame.", } pose_covariance_diagonal: { From ca5a81ccc48be48fd0dcc9f3754f52780a1d991a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 12 Jan 2023 09:31:06 +0000 Subject: [PATCH 05/21] Update changelogs --- admittance_controller/CHANGELOG.rst | 5 +++++ diff_drive_controller/CHANGELOG.rst | 6 ++++++ effort_controllers/CHANGELOG.rst | 5 +++++ force_torque_sensor_broadcaster/CHANGELOG.rst | 5 +++++ forward_command_controller/CHANGELOG.rst | 5 +++++ gripper_controllers/CHANGELOG.rst | 5 +++++ imu_sensor_broadcaster/CHANGELOG.rst | 5 +++++ joint_state_broadcaster/CHANGELOG.rst | 5 +++++ joint_trajectory_controller/CHANGELOG.rst | 5 +++++ position_controllers/CHANGELOG.rst | 5 +++++ ros2_controllers/CHANGELOG.rst | 3 +++ ros2_controllers_test_nodes/CHANGELOG.rst | 3 +++ rqt_joint_trajectory_controller/CHANGELOG.rst | 5 +++++ tricycle_controller/CHANGELOG.rst | 5 +++++ velocity_controllers/CHANGELOG.rst | 5 +++++ 15 files changed, 72 insertions(+) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index cc7fa5f2bf..8475b7e077 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 3546af9bba..91d4825e64 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* diff_drive base_frame_id param (`#495 `_) (`#498 `_) +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar, Jakub Delicat + 2.15.0 (2022-12-06) ------------------- * [DiffDriveController] Use generate parameter library (`#386 `_) diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 6954b1f7e3..3f290aafd1 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index 3a37a17174..dfc1d7306f 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 0dc1db853b..367da82ad5 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index e11321aefe..9ebc2da041 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- * Add basic gripper controller tests (`#459 `_) diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 0f26b4cbf2..5532738711 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 68d25c05aa..43eb4f3255 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index bc59ab4960..8c37b20f0b 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 115fac4e37..28337ac656 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 0799e154c7..a44b6f10b0 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index f60cb9d72a..6195129137 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.15.0 (2022-12-06) ------------------- diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index d4b07df08b..71c020fcc3 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) (`#496 `_) +* Contributors: Denis Stogl + 2.15.0 (2022-12-06) ------------------- diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 0739bf6446..f79b9e766f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- * [TricycleController] Removed “publish period” functionality ⏱ #abi-break #behavior-break (`#468 `_) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e47e52cdac..e0d75c90e1 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add backward_ros to all controllers (`#489 `_) (`#493 `_) +* Contributors: Bence Magyar + 2.15.0 (2022-12-06) ------------------- From 85dac2a6c65e777fa04caf6af15a41530bbbac6b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 19 Jan 2023 12:32:46 +0000 Subject: [PATCH 06/21] 2.16.0 --- admittance_controller/CHANGELOG.rst | 4 ++-- admittance_controller/package.xml | 2 +- diff_drive_controller/CHANGELOG.rst | 4 ++-- diff_drive_controller/package.xml | 2 +- effort_controllers/CHANGELOG.rst | 4 ++-- effort_controllers/package.xml | 2 +- force_torque_sensor_broadcaster/CHANGELOG.rst | 4 ++-- force_torque_sensor_broadcaster/package.xml | 2 +- forward_command_controller/CHANGELOG.rst | 4 ++-- forward_command_controller/package.xml | 2 +- gripper_controllers/CHANGELOG.rst | 4 ++-- gripper_controllers/package.xml | 2 +- imu_sensor_broadcaster/CHANGELOG.rst | 4 ++-- imu_sensor_broadcaster/package.xml | 2 +- joint_state_broadcaster/CHANGELOG.rst | 4 ++-- joint_state_broadcaster/package.xml | 2 +- joint_trajectory_controller/CHANGELOG.rst | 4 ++-- joint_trajectory_controller/package.xml | 2 +- position_controllers/CHANGELOG.rst | 4 ++-- position_controllers/package.xml | 2 +- ros2_controllers/CHANGELOG.rst | 4 ++-- ros2_controllers/package.xml | 2 +- ros2_controllers_test_nodes/CHANGELOG.rst | 4 ++-- ros2_controllers_test_nodes/package.xml | 2 +- ros2_controllers_test_nodes/setup.py | 2 +- rqt_joint_trajectory_controller/CHANGELOG.rst | 4 ++-- rqt_joint_trajectory_controller/package.xml | 2 +- rqt_joint_trajectory_controller/setup.py | 2 +- tricycle_controller/CHANGELOG.rst | 4 ++-- tricycle_controller/package.xml | 2 +- velocity_controllers/CHANGELOG.rst | 4 ++-- velocity_controllers/package.xml | 2 +- 32 files changed, 47 insertions(+), 47 deletions(-) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 8475b7e077..b39e68f6cf 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 69180a290f..cbd4e8fe69 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 2.15.0 + 2.16.0 Implementation of admittance controllers for different input and output interface. Denis Å togl Bence Magyar diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 91d4825e64..25c46b39c8 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * diff_drive base_frame_id param (`#495 `_) (`#498 `_) * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar, Jakub Delicat diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index 4feec7fe37..efbed7275f 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 2.15.0 + 2.16.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 3f290aafd1..4342b72054 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 0ca0d8dffb..8f1fec01c8 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 2.15.0 + 2.16.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 dfc1d7306f..a0b5987283 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 174499c573..112f96ec64 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 2.15.0 + 2.16.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Å togl diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 367da82ad5..2f946b9770 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index f6ded4ff93..612f7025b3 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 2.15.0 + 2.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 9ebc2da041..4e2df22de5 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 2734c54933..57f4103186 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 2.15.0 + 2.16.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 5532738711..0860898080 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 40836967c3..311cc9d4d6 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 2.15.0 + 2.16.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Å togl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 43eb4f3255..0390fea1e3 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 9fd5942d5d..b439073e9c 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 2.15.0 + 2.16.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 8c37b20f0b..2597aee2c7 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index a589768776..a142573d84 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 2.15.0 + 2.16.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Jordan Palacios diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 28337ac656..27a6e6b97e 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 086afccc1f..e7b47e3ed8 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 2.15.0 + 2.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index a44b6f10b0..c0612cbd9c 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index e91679802c..40bacb9edc 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 2.15.0 + 2.16.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index 6195129137..f578f38a07 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- 2.15.0 (2022-12-06) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 269d1da466..2243905f88 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 2.15.0 + 2.16.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Å togl diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index ed3ae63656..a23d99e206 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="2.15.0", + version="2.16.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 71c020fcc3..c3116b8570 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * 🔧 Fixes and updated on pre-commit hooks and their action (`#492 `_) (`#496 `_) * Contributors: Denis Stogl diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 81e69f88fd..096f822988 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 - 2.15.0 + 2.16.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index afb93527f1..a1d67a4c6a 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version="2.15.0", + version="2.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index f79b9e766f..8a88b34a0f 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index 3c853872e9..e4147fe2c5 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 2.15.0 + 2.16.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index e0d75c90e1..996d42904e 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.16.0 (2023-01-19) +------------------- * Add backward_ros to all controllers (`#489 `_) (`#493 `_) * Contributors: Bence Magyar diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 03f1bed657..c746c8101a 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 2.15.0 + 2.16.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios From e4a43e5a1c3a6328e7bf38c684e1e2998d2b13ef Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Mon, 30 Jan 2023 17:47:43 +0100 Subject: [PATCH 07/21] 4 wheel controller --- four_steering_controller/CMakeLists.txt | 76 +++ .../four_steering_controller.xml | 7 + .../four_steering_controller.hpp | 153 ++++++ .../four_steering_controller/odometry.hpp | 66 +++ .../speed_limiter.hpp | 87 ++++ .../visibility_control.h | 56 +++ four_steering_controller/package.xml | 35 ++ .../src/four_steering_controller.cpp | 449 ++++++++++++++++++ .../four_steering_controller_parameter.yaml | 46 ++ four_steering_controller/src/interval.cpp | 172 +++++++ four_steering_controller/src/odometry.cpp | 145 ++++++ .../src/speed_limiter.cpp | 122 +++++ 12 files changed, 1414 insertions(+) create mode 100644 four_steering_controller/CMakeLists.txt create mode 100644 four_steering_controller/four_steering_controller.xml create mode 100644 four_steering_controller/include/four_steering_controller/four_steering_controller.hpp create mode 100644 four_steering_controller/include/four_steering_controller/odometry.hpp create mode 100644 four_steering_controller/include/four_steering_controller/speed_limiter.hpp create mode 100644 four_steering_controller/include/four_steering_controller/visibility_control.h create mode 100644 four_steering_controller/package.xml create mode 100644 four_steering_controller/src/four_steering_controller.cpp create mode 100644 four_steering_controller/src/four_steering_controller_parameter.yaml create mode 100644 four_steering_controller/src/interval.cpp create mode 100644 four_steering_controller/src/odometry.cpp create mode 100644 four_steering_controller/src/speed_limiter.cpp diff --git a/four_steering_controller/CMakeLists.txt b/four_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..d00ec75a1a --- /dev/null +++ b/four_steering_controller/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.5) +project(four_steering_controller) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +find_package(generate_parameter_library REQUIRED) + +generate_parameter_library(four_steering_controller_parameters + src/four_steering_controller_parameter.yaml +) + +add_library(${PROJECT_NAME} SHARED + src/four_steering_controller.cpp + src/odometry.cpp + src/speed_limiter.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC $ + $) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(${PROJECT_NAME} + four_steering_controller_parameters +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "FOUR_STEERING_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface four_steering_controller.xml) + +install(DIRECTORY include/ + DESTINATION include +) + +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_package() diff --git a/four_steering_controller/four_steering_controller.xml b/four_steering_controller/four_steering_controller.xml new file mode 100644 index 0000000000..206063fe05 --- /dev/null +++ b/four_steering_controller/four_steering_controller.xml @@ -0,0 +1,7 @@ + + + + The four steering drive controller transforms linear and angular velocity messages into signals for each wheel(s) and steer(s). + + + diff --git a/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp b/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp new file mode 100644 index 0000000000..58a156c162 --- /dev/null +++ b/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp @@ -0,0 +1,153 @@ +#ifndef FOUR_STEERING_CONTROLLER__FOUR_STEERING_CONTROLLER_HPP_ +#define FOUR_STEERING_CONTROLLER__FOUR_STEERING_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "four_steering_controller/odometry.hpp" +#include "four_steering_controller/speed_limiter.hpp" +#include "four_steering_controller/visibility_control.h" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "hardware_interface/handle.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_box.h" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "tf2_msgs/msg/tf_message.hpp" + +#include "four_steering_controller_parameters.hpp" + +namespace four_steering_controller +{ +class FourSteeringController : public controller_interface::ControllerInterface +{ + using Twist = geometry_msgs::msg::TwistStamped; + +public: + FOUR_STEERING_CONTROLLER_PUBLIC + FourSteeringController(); + + FOUR_STEERING_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + FOUR_STEERING_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + FOUR_STEERING_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + FOUR_STEERING_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + FOUR_STEERING_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + FOUR_STEERING_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + FOUR_STEERING_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + FOUR_STEERING_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + FOUR_STEERING_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + + FOUR_STEERING_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + +protected: + // Parte nuova: + struct SteerHandle + { + std::reference_wrapper feedback; + std::reference_wrapper velocity; + }; + struct WheelHandle + { + std::reference_wrapper feedback; + std::reference_wrapper velocity; + }; + + const char * feedback_type() const; + + controller_interface::CallbackReturn configure_wheels( + const std::vector & wheel_names, + std::vector & registered_handles); + + controller_interface::CallbackReturn configure_steers( + const std::vector & steer_names, + std::vector & registered_handles); + + std::vector registered_wheel_handles_; + std::vector registered_steer_handles_; + + // Parameters from ROS for diff_drive_controller + std::shared_ptr param_listener_; + Params params_; + + Odometry odometry_; + + // Timeout to consider cmd_vel commands old + std::chrono::milliseconds cmd_vel_timeout_{500}; + + std::shared_ptr> odometry_publisher_ = nullptr; + std::shared_ptr> + realtime_odometry_publisher_ = nullptr; + + std::shared_ptr> odometry_transform_publisher_ = + nullptr; + std::shared_ptr> + realtime_odometry_transform_publisher_ = nullptr; + + bool subscriber_is_active_ = false; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr + velocity_command_unstamped_subscriber_ = nullptr; + + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + + std::queue previous_commands_; // last two commands + + // speed limiters + SpeedLimiter limiter_linear_; + SpeedLimiter limiter_angular_; + + bool publish_limited_velocity_ = false; + std::shared_ptr> limited_velocity_publisher_ = nullptr; + std::shared_ptr> realtime_limited_velocity_publisher_ = + nullptr; + + rclcpp::Time previous_update_timestamp_{0}; + + // publish rate limiter + double publish_rate_ = 50.0; + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + rclcpp::Time previous_publish_timestamp_{0}; + + bool is_halted = false; + bool use_stamped_vel_ = true; + std::vector wheel_vel_cmd {0.0}; + std::vector steer_vel_cmd {0.0}; + bool reset(); + void halt(); +}; +} // namespace four_steering_controller +#endif // FOUR_STEERING_CONTROLLER__FOUR_STEERING_CONTROLLER_HPP_ diff --git a/four_steering_controller/include/four_steering_controller/odometry.hpp b/four_steering_controller/include/four_steering_controller/odometry.hpp new file mode 100644 index 0000000000..d04280af5e --- /dev/null +++ b/four_steering_controller/include/four_steering_controller/odometry.hpp @@ -0,0 +1,66 @@ +#ifndef FOUR_STEERING_CONTROLLER__ODOMETRY_HPP_ +#define FOUR_STEERING_CONTROLLER__ODOMETRY_HPP_ + +#include + +#include "rclcpp/time.hpp" +#include "rcppmath/rolling_mean_accumulator.hpp" + +namespace four_steering_controller +{ +class Odometry +{ +public: + explicit Odometry(size_t velocity_rolling_window_size = 10); + + void init(const rclcpp::Time & time); + bool update(double left_pos, double right_pos, const rclcpp::Time & time); + bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time); + void updateOpenLoop(double linear, double angular, const rclcpp::Time & time); + void resetOdometry(); + + double getX() const { return x_; } + double getY() const { return y_; } + double getHeading() const { return heading_; } + double getLinear() const { return linear_; } + double getAngular() const { return angular_; } + + void setWheelParams(double wheel_separation, double wheel_radius); + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + +private: + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; + + void integrateRungeKutta2(double linear, double angular); + void integrateExact(double linear, double angular); + void resetAccumulators(); + + // Current timestamp: + rclcpp::Time timestamp_; + + // Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + // Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + // Wheel kinematic parameters [m]: + double wheel_separation_; + double wheel_radius_; + + // Previous wheel position/state [rad]: + double left_wheel_old_pos_; + double right_wheel_old_pos_; + + // Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAccumulator linear_accumulator_; + RollingMeanAccumulator angular_accumulator_; +}; + +} // namespace four_steering_controller + +#endif // FOUR_STEERING_CONTROLLER__ODOMETRY_HPP_ diff --git a/four_steering_controller/include/four_steering_controller/speed_limiter.hpp b/four_steering_controller/include/four_steering_controller/speed_limiter.hpp new file mode 100644 index 0000000000..6a13f8db66 --- /dev/null +++ b/four_steering_controller/include/four_steering_controller/speed_limiter.hpp @@ -0,0 +1,87 @@ +#ifndef FOUR_STEERING_CONTROLLER__SPEED_LIMITER_HPP_ +#define FOUR_STEERING_CONTROLLER__SPEED_LIMITER_HPP_ + +#include + +namespace four_steering_controller +{ +class SpeedLimiter +{ +public: + /** + * \brief Constructor + * \param [in] has_velocity_limits if true, applies velocity limits + * \param [in] has_acceleration_limits if true, applies acceleration limits + * \param [in] has_jerk_limits if true, applies jerk limits + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + SpeedLimiter( + bool has_velocity_limits = false, bool has_acceleration_limits = false, + bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, + double max_jerk = NAN); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & v, double v0, double v1, double dt); + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & v); + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & v, double v0, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control + */ + double limit_jerk(double & v, double v0, double v1, double dt); + +private: + // Enable/Disable velocity/acceleration/jerk limits: + bool has_velocity_limits_; + bool has_acceleration_limits_; + bool has_jerk_limits_; + + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; + + // Jerk limits: + double min_jerk_; + double max_jerk_; +}; + +} // namespace four_steering_controller + +#endif // FOUR_STEERING_CONTROLLER__SPEED_LIMITER_HPP_ diff --git a/four_steering_controller/include/four_steering_controller/visibility_control.h b/four_steering_controller/include/four_steering_controller/visibility_control.h new file mode 100644 index 0000000000..46248acfd6 --- /dev/null +++ b/four_steering_controller/include/four_steering_controller/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef FOUR_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define FOUR_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 FOUR_STEERING_CONTROLLER_EXPORT __attribute__((dllexport)) +#define FOUR_STEERING_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define FOUR_STEERING_CONTROLLER_EXPORT __declspec(dllexport) +#define FOUR_STEERING_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef FOUR_STEERING_CONTROLLER_BUILDING_DLL +#define FOUR_STEERING_CONTROLLER_PUBLIC FOUR_STEERING_CONTROLLER_EXPORT +#else +#define FOUR_STEERING_CONTROLLER_PUBLIC FOUR_STEERING_CONTROLLER_IMPORT +#endif +#define FOUR_STEERING_CONTROLLER_PUBLIC_TYPE FOUR_STEERING_CONTROLLER_PUBLIC +#define FOUR_STEERING_CONTROLLER_LOCAL +#else +#define FOUR_STEERING_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define FOUR_STEERING_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define FOUR_STEERING_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define FOUR_STEERING_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define FOUR_STEERING_CONTROLLER_PUBLIC +#define FOUR_STEERING_CONTROLLER_LOCAL +#endif +#define FOUR_STEERING_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // FOUR_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/four_steering_controller/package.xml b/four_steering_controller/package.xml new file mode 100644 index 0000000000..07735ede5b --- /dev/null +++ b/four_steering_controller/package.xml @@ -0,0 +1,35 @@ + + + four_steering_controller + 2.15.0 + Controller for a 4 steering drive mobile base. + Bence Magyar + Jordan Palacios + + Apache License 2.0 + + ament_cmake + generate_parameter_library + + backward_ros + controller_interface + geometry_msgs + hardware_interface + nav_msgs + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs + + pluginlib + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/four_steering_controller/src/four_steering_controller.cpp b/four_steering_controller/src/four_steering_controller.cpp new file mode 100644 index 0000000000..08aa2a10f8 --- /dev/null +++ b/four_steering_controller/src/four_steering_controller.cpp @@ -0,0 +1,449 @@ +#include +#include +#include +#include +#include + +#include "four_steering_controller/four_steering_controller.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" +#include "tf2/LinearMath/Quaternion.h" + +namespace +{ +constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; +constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; +} // namespace + +namespace four_steering_controller +{ +using namespace std::chrono_literals; +using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using lifecycle_msgs::msg::State; + + +FourSteeringController::FourSteeringController() : controller_interface::ControllerInterface() {} + +const char * FourSteeringController::feedback_type() const +{ + return params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; +} + +controller_interface::CallbackReturn FourSteeringController::on_init() +{ + try + { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + auto log = get_node()->get_logger(); + RCLCPP_WARN(log, "Parameters read correctly."); + + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +// Get joint names from the parameter +InterfaceConfiguration FourSteeringController::command_interface_configuration() const +{ + std::vector conf_names; + for (const auto & joint_name : params_.wheel_names) + { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + } + for (const auto & joint_name : params_.steer_names) + { + conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + } + RCLCPP_WARN(get_node()->get_logger(), "Command interfaces set."); + + return {interface_configuration_type::INDIVIDUAL, conf_names}; + +} + +InterfaceConfiguration FourSteeringController::state_interface_configuration() const +{ + std::vector conf_joint_names; + for (const auto & joint_name : params_.wheel_names) + { + conf_joint_names.push_back(joint_name + "/" + feedback_type()); + } + for (const auto & joint_name : params_.steer_names) + { + conf_joint_names.push_back(joint_name + "/" + feedback_type()); + } + RCLCPP_WARN(get_node()->get_logger(), "State interfaces set."); + + return {interface_configuration_type::INDIVIDUAL, conf_joint_names}; +} + + +controller_interface::return_type FourSteeringController::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + auto logger = get_node()->get_logger(); + if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted) + { + halt(); + is_halted = true; + } + return controller_interface::return_type::OK; + } + std::shared_ptr last_command_msg; + received_velocity_msg_ptr_.get(last_command_msg); + + if (last_command_msg == nullptr) + { + RCLCPP_WARN(logger, "Velocity message received was a nullptr."); + return controller_interface::return_type::ERROR; + } + + const auto age_of_last_command = time - last_command_msg->header.stamp; + + // Brake if cmd_vel has timeout, override the stored command + if (age_of_last_command > cmd_vel_timeout_) + { + last_command_msg->twist.linear.x = 0.0; + last_command_msg->twist.angular.z = 0.0; + } + Twist command = *last_command_msg; + double & linear_cmd = command.twist.linear.x; + double & angular_cmd = command.twist.angular.z; + + // check if OpenLoop condition is met. + RCLCPP_WARN(get_node()->get_logger(), "Twist Correctly stored."); + + const double steering_track = params_.track_ - 2 * params_.wheel_steering_y_offset_; + RCLCPP_WARN(get_node()->get_logger(), "Steering track value: %f", steering_track); + + + const double joint_number = params_.wheel_names.size(); + double i = -1.0; + // Compute wheels velocities: + if(fabs(linear_cmd) > 0.001) + { + const double vel_steering_offset = (angular_cmd * params_.wheel_steering_y_offset_)/params_.wheel_radius_; + const double sign = copysign(1.0, linear_cmd); + RCLCPP_WARN(get_node()->get_logger(), "vel_steering_offset: %f", vel_steering_offset); + + for(size_t wheel=0; wheel < joint_number; wheel++) + { //front_l - front_r - rear_l - rear_r + wheel_vel_cmd[wheel] = sign * std::hypot((linear_cmd + (i) * angular_cmd*steering_track/2), + (params_.wheel_base_*angular_cmd/2.0)) / params_.wheel_radius_ + (i) * vel_steering_offset; + i *= -1.0; + RCLCPP_WARN(get_node()->get_logger(), "Wheel_cmd[%d]: %f", i,wheel_vel_cmd[wheel]); + + } + } + + // Compute steering angles + if(fabs(2.0*linear_cmd) > fabs(angular_cmd*steering_track)) + { + for(size_t steer=0; steer < joint_number; steer++) { + // front_l - rear_l - front_r - rear_r + steer_vel_cmd[steer] = (-i) * atan(angular_cmd*params_.wheel_base_ / + (2.0 * linear_cmd + (i) * angular_cmd * steering_track)); + i *= -1.0; + } + } + + // Set wheels velocities: + for (size_t index=0; index < joint_number; index++) + { + registered_wheel_handles_[index].velocity.get().set_value(wheel_vel_cmd[index]); + } + // Set steers velocities: + for (size_t index=0; index < joint_number; index++) + { + registered_steer_handles_[index].velocity.get().set_value(steer_vel_cmd[index]); + } + return controller_interface::return_type::OK; + +} + +controller_interface::CallbackReturn FourSteeringController::on_configure( + const rclcpp_lifecycle::State &) +{ + auto logger = get_node()->get_logger(); + + // update parameters if they have changed + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + RCLCPP_INFO(logger, "Parameters were updated"); + } + + if (params_.wheel_names.empty()) + { + RCLCPP_ERROR( + logger, "The wheels parameter can't be empty. Wheels_names size: [%zu]", params_.wheel_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + + const double wheel_separation = params_.track_; + const double wheel_radius = params_.wheel_radius_; + const double wheel_steering_y_offset_ = params_.wheel_steering_y_offset_; + + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + + const Twist empty_twist; + received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); + RCLCPP_WARN(get_node()->get_logger(), "Empty Twist Command created."); + // Fill last two commands with default constructed commands + previous_commands_.emplace(empty_twist); + previous_commands_.emplace(empty_twist); + + // initialize command subscriber + velocity_command_unstamped_subscriber_ = + get_node()->create_subscription( + DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + RCLCPP_WARN(get_node()->get_logger(), "Entered the lambda function."); + + if (!subscriber_is_active_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + // Write fake header in the stored stamped command + std::shared_ptr twist_stamped; + received_velocity_msg_ptr_.get(twist_stamped); + twist_stamped->twist = *msg; + twist_stamped->header.stamp = get_node()->get_clock()->now(); + std::cout << "Msg created all good." << std::endl; + + } + ); + + previous_update_timestamp_ = get_node()->get_clock()->now(); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn FourSteeringController::on_activate(const rclcpp_lifecycle::State &) +{ + std::cout << "On_activate: configuring wheels and steers. " << std::endl; + + const auto wheels_result = + configure_wheels(params_.wheel_names, registered_wheel_handles_); + const auto steers_result = + configure_steers(params_.steer_names, registered_steer_handles_); + + if ( + wheels_result == controller_interface::CallbackReturn::ERROR || + steers_result == controller_interface::CallbackReturn::ERROR) + { + return controller_interface::CallbackReturn::ERROR; + } + + if (registered_wheel_handles_.empty() || registered_steer_handles_.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Either wheel interfaces, steer interfaces are non existent"); + return controller_interface::CallbackReturn::ERROR; + } + + is_halted = false; + subscriber_is_active_ = true; + + RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn FourSteeringController::on_deactivate( + const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn FourSteeringController::on_cleanup( + const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + + received_velocity_msg_ptr_.set(std::make_shared()); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn FourSteeringController::on_error(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +bool FourSteeringController::reset() +{ + + // release the old queue + std::queue empty; + std::swap(previous_commands_, empty); + registered_wheel_handles_.clear(); + registered_steer_handles_.clear(); + + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + velocity_command_unstamped_subscriber_.reset(); + + received_velocity_msg_ptr_.set(nullptr); + is_halted = false; + + return true; +} + +controller_interface::CallbackReturn FourSteeringController::on_shutdown( + const rclcpp_lifecycle::State &) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +void FourSteeringController::halt() +{ + const auto halt_wheels = [](auto & wheel_handles) + { + for (const auto & wheel_handle : wheel_handles) + { + wheel_handle.velocity.get().set_value(0.0); + } + }; + + halt_wheels(registered_wheel_handles_); + +} + +controller_interface::CallbackReturn FourSteeringController::configure_wheels( + const std::vector & wheel_names, + std::vector & registered_handles) +{ + auto logger = get_node()->get_logger(); + + if (wheel_names.empty()) + { + RCLCPP_ERROR(logger, "No wheel names specified"); + return controller_interface::CallbackReturn::ERROR; + } + + // register handles + registered_handles.reserve(wheel_names.size()); + for (const auto & wheel_name : wheel_names) + { + const auto interface_name = feedback_type(); + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&wheel_name, &interface_name](const auto & interface) + { + return interface.get_prefix_name() == wheel_name && + interface.get_interface_name() == interface_name; + }); + + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&wheel_name](const auto & interface) + { + return interface.get_prefix_name() == wheel_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + registered_handles.emplace_back( + WheelHandle{std::ref(*state_handle), std::ref(*command_handle)}); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + + +controller_interface::CallbackReturn FourSteeringController::configure_steers( + const std::vector & steer_names, + std::vector & registered_handles) +{ + auto logger = get_node()->get_logger(); + + if (steer_names.empty()) + { + RCLCPP_ERROR(logger, "No steer names specified"); + return controller_interface::CallbackReturn::ERROR; + } + + // register handles + registered_handles.reserve(steer_names.size()); + for (const auto & steer_name : steer_names) + { + const auto interface_name = feedback_type(); + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&steer_name, &interface_name](const auto & interface) + { + return interface.get_prefix_name() == steer_name && + interface.get_interface_name() == interface_name; + }); + + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", steer_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&steer_name](const auto & interface) + { + return interface.get_prefix_name() == steer_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", steer_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + + registered_handles.emplace_back( + SteerHandle{std::ref(*state_handle), std::ref(*command_handle)}); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +} // namespace four_steering_controller +#include "class_loader/register_macro.hpp" + +CLASS_LOADER_REGISTER_CLASS( + four_steering_controller::FourSteeringController, controller_interface::ControllerInterface) diff --git a/four_steering_controller/src/four_steering_controller_parameter.yaml b/four_steering_controller/src/four_steering_controller_parameter.yaml new file mode 100644 index 0000000000..5b77798b39 --- /dev/null +++ b/four_steering_controller/src/four_steering_controller_parameter.yaml @@ -0,0 +1,46 @@ +four_steering_controller: + wheel_names: { + type: string_array, + default_value: [], + description: "Link names of the wheel joints.", + } + steer_names: { + type: string_array, + default_value: [], + description: "Link names of the steer joints.", + } + wheel_steering_y_offset_: { + type: double, + default_value: 0.15, + description: "Distance from mid-point of the wheel width and the associated steer joint.", + } + wheel_radius_: { + type: double, + default_value: 1.0, + description: "Radius of a wheel.", + } + wheel_base_: { + type: double, + default_value: 1.0, + description: "Distance between fron and rear wheels.", + } + track_: { + type: double, + default_value: 1.0, + description: "Distance between right and left wheels. (Mid-point of the wheel width).", + } + position_feedback: { + type: bool, + default_value: true, + description: "Is there position feedback from hardware.", + } + cmd_vel_timeout: { + type: double, + default_value: 4.0, # Hz + description: "Timeout on cmd_vel command.", + } + publish_rate: { + type: double, + default_value: 50.0, # Hz + description: "Publishing rate (Hz) of the odometry and TF messages.", + } \ No newline at end of file diff --git a/four_steering_controller/src/interval.cpp b/four_steering_controller/src/interval.cpp new file mode 100644 index 0000000000..400001e72c --- /dev/null +++ b/four_steering_controller/src/interval.cpp @@ -0,0 +1,172 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Mark Naeem + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * The names of the contributors may NOT be used to endorse or + * promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Mark Naeem + */ + +#include + +interval::interval(const std::array &limits, const std::string &type) +{ + const auto& types_val = types_dict_[type]; + this->limits_.push_back(limits); + this->types_.push_back(types_val); +} + + +bool interval::intersect(std::array& limits1, const std::array& limits2, std::array& type1, const std::array& type2) +{ + bool interval1_point, interval2_point = false; + if (limits1[0] == limits1[1]) interval1_point = true; + if (limits2[0] == limits2[1]) interval2_point = true; + + if (interval1_point && interval2_point) + return (limits1[0]==limits2[0])? true: false; + + else if (interval1_point && !interval2_point) + { + if (limits1[0]==limits2[0]) + return (type2[0]==0)?false:true; + else if (limits1[0]==limits2[1]) + return (type2[1]==0)?false:true; + else if (limits1[0]>limits2[0] && limits1[0]limits1[0] && limits2[0]limits_.size(); + const auto size2 = another_interval.limits_.size(); + + if (size1==1 && size2==1) + return intersect(limits_[0], another_interval.limits_[0], types_[0], another_interval.types_[0]); + else if (size1==1 && size2==2) + { + const auto& b1 = intersect(limits_[0], another_interval.limits_[0], types_[0], another_interval.types_[0]); + const auto& b2 = intersect(limits_[0], another_interval.limits_[1], types_[0], another_interval.types_[1]); + return b1 || b2; + } + else if (size1==2 && size2==1) + { + const auto& b1 = intersect(limits_[0], another_interval.limits_[0], types_[0], another_interval.types_[0]); + const auto& b2 = intersect(limits_[1], another_interval.limits_[0], types_[1], another_interval.types_[0]); + return b1 || b2; + } + else + { + const auto& b1 = intersect(limits_[0], another_interval.limits_[0], types_[0], another_interval.types_[0]); + const auto& b2 = intersect(limits_[1], another_interval.limits_[0], types_[1], another_interval.types_[0]); + const auto& b3 = intersect(limits_[0], another_interval.limits_[1], types_[0], another_interval.types_[1]); + const auto& b4 = intersect(limits_[1], another_interval.limits_[1], types_[1], another_interval.types_[1]); + return b1 || b2 || b3 || b4; + } +} + +interval interval::complement () const +{ + /*if (limits_.size()>1) //do something because we can't do it yet*/ + interval complementary_interval(std::array{-(M_PI+0.1),limits_[0][0]}, types_[0][0]==1 ? "closeopen" : "close"); + complementary_interval.limits_.push_back(std::array{limits_[0][1],(M_PI+0.1)}); + complementary_interval.types_.push_back(types_[0][1]==1 ? std::array{0,1} : std::array{1,1}); + return complementary_interval; +} + +double interval::len() const +{ + double res = 0; + for (auto it: limits_) // we need this to be a copy not a reference + { + if (it[0]==-(M_PI+0.1)) it[0] =-M_PI; + if (it[1]== (M_PI+0.1)) it[1] = M_PI; + res += fabs(it[0]-it[1]); + } + return res; +} + +void interval::print()const +{ + for (const auto& it: limits_) + { + std::cout<<"[ "< +#include + +#include "four_steering_controller/speed_limiter.hpp" +#include "rcppmath/clamp.hpp" + +namespace four_steering_controller +{ +SpeedLimiter::SpeedLimiter( + bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity, + double max_velocity, double min_acceleration, double max_acceleration, double min_jerk, + double max_jerk) +: has_velocity_limits_(has_velocity_limits), + has_acceleration_limits_(has_acceleration_limits), + has_jerk_limits_(has_jerk_limits), + min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration), + min_jerk_(min_jerk), + max_jerk_(max_jerk) +{ + // Check if limits are valid, max must be specified, min defaults to -max if unspecified + if (has_velocity_limits_) + { + if (std::isnan(max_velocity_)) + { + throw std::runtime_error("Cannot apply velocity limits if max_velocity is not specified"); + } + if (std::isnan(min_velocity_)) + { + min_velocity_ = -max_velocity_; + } + } + if (has_acceleration_limits_) + { + if (std::isnan(max_acceleration_)) + { + throw std::runtime_error( + "Cannot apply acceleration limits if max_acceleration is not specified"); + } + if (std::isnan(min_acceleration_)) + { + min_acceleration_ = -max_acceleration_; + } + } + if (has_jerk_limits_) + { + if (std::isnan(max_jerk_)) + { + throw std::runtime_error("Cannot apply jerk limits if max_jerk is not specified"); + } + if (std::isnan(min_jerk_)) + { + min_jerk_ = -max_jerk_; + } + } +} + +double SpeedLimiter::limit(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + limit_jerk(v, v0, v1, dt); + limit_acceleration(v, v0, dt); + limit_velocity(v); + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_velocity(double & v) +{ + const double tmp = v; + + if (has_velocity_limits_) + { + v = rcppmath::clamp(v, min_velocity_, max_velocity_); + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_acceleration(double & v, double v0, double dt) +{ + const double tmp = v; + + if (has_acceleration_limits_) + { + const double dv_min = min_acceleration_ * dt; + const double dv_max = max_acceleration_ * dt; + + const double dv = rcppmath::clamp(v - v0, dv_min, dv_max); + + v = v0 + dv; + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + if (has_jerk_limits_) + { + const double dv = v - v0; + const double dv0 = v0 - v1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_jerk_ * dt2; + const double da_max = max_jerk_ * dt2; + + const double da = rcppmath::clamp(dv - dv0, da_min, da_max); + + v = v0 + dv0 + da; + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +} // namespace four_steering_controller From 60de7bf814cd5357d091bbe98770bf2a78db7a6a Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Tue, 7 Feb 2023 16:12:13 +0100 Subject: [PATCH 08/21] Update using the 4-wheels-steering message. --- four_steering_controller/CMakeLists.txt | 1 + .../four_steering_controller.hpp | 40 +- four_steering_controller/package.xml | 2 +- .../src/four_steering_controller.cpp | 350 +++++++++++++----- 4 files changed, 291 insertions(+), 102 deletions(-) diff --git a/four_steering_controller/CMakeLists.txt b/four_steering_controller/CMakeLists.txt index d00ec75a1a..b4d4486b27 100644 --- a/four_steering_controller/CMakeLists.txt +++ b/four_steering_controller/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS realtime_tools tf2 tf2_msgs + four_wheel_steering_msgs ) find_package(ament_cmake REQUIRED) diff --git a/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp b/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp index 58a156c162..2410f41553 100644 --- a/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp +++ b/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp @@ -25,6 +25,7 @@ #include "tf2_msgs/msg/tf_message.hpp" #include "four_steering_controller_parameters.hpp" +#include "four_wheel_steering_msgs/msg/four_wheel_steering_stamped.hpp" namespace four_steering_controller { @@ -73,21 +74,25 @@ class FourSteeringController : public controller_interface::ControllerInterface controller_interface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & previous_state) override; + template + T clamp(T x, T min, T max) + { + return std::min(std::max(min, x), max); + }; + protected: // Parte nuova: struct SteerHandle { - std::reference_wrapper feedback; - std::reference_wrapper velocity; + std::reference_wrapper feedback_pos; + std::reference_wrapper position; }; struct WheelHandle { std::reference_wrapper feedback; std::reference_wrapper velocity; }; - - const char * feedback_type() const; - + controller_interface::CallbackReturn configure_wheels( const std::vector & wheel_names, std::vector & registered_handles); @@ -118,21 +123,21 @@ class FourSteeringController : public controller_interface::ControllerInterface realtime_odometry_transform_publisher_ = nullptr; bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr velocity_command_unstamped_subscriber_ = nullptr; - realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; - std::queue previous_commands_; // last two commands + std::queue previous_commands_; // last two commands // speed limiters SpeedLimiter limiter_linear_; SpeedLimiter limiter_angular_; bool publish_limited_velocity_ = false; - std::shared_ptr> limited_velocity_publisher_ = nullptr; - std::shared_ptr> realtime_limited_velocity_publisher_ = + std::shared_ptr> limited_velocity_publisher_ = nullptr; + std::shared_ptr> realtime_limited_velocity_publisher_ = nullptr; rclcpp::Time previous_update_timestamp_{0}; @@ -144,8 +149,17 @@ class FourSteeringController : public controller_interface::ControllerInterface bool is_halted = false; bool use_stamped_vel_ = true; - std::vector wheel_vel_cmd {0.0}; - std::vector steer_vel_cmd {0.0}; + + double wheel_vel_cmd[4] = { 0 }; + double steer_cmd[4] = { 0 }; + double alpha_read[4] = { 0 }; + double w_speed[4] = { 0 }; + bool pivot = false; + double vel_left_front = 0.0, vel_right_front = 0.0; + double vel_left_rear = 0.0, vel_right_rear = 0.0; + double front_left_steering = 0.0, front_right_steering = 0.0; + double rear_left_steering = 0.0, rear_right_steering = 0.0; + double wheel_vel = 0.0; bool reset(); void halt(); }; diff --git a/four_steering_controller/package.xml b/four_steering_controller/package.xml index 07735ede5b..64d49d9bf1 100644 --- a/four_steering_controller/package.xml +++ b/four_steering_controller/package.xml @@ -22,7 +22,7 @@ realtime_tools tf2 tf2_msgs - + four_wheel_steering_msgs pluginlib ament_cmake_gmock diff --git a/four_steering_controller/src/four_steering_controller.cpp b/four_steering_controller/src/four_steering_controller.cpp index 08aa2a10f8..0f7eb8de16 100644 --- a/four_steering_controller/src/four_steering_controller.cpp +++ b/four_steering_controller/src/four_steering_controller.cpp @@ -31,11 +31,6 @@ using lifecycle_msgs::msg::State; FourSteeringController::FourSteeringController() : controller_interface::ControllerInterface() {} -const char * FourSteeringController::feedback_type() const -{ - return params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY; -} - controller_interface::CallbackReturn FourSteeringController::on_init() { try @@ -43,8 +38,7 @@ controller_interface::CallbackReturn FourSteeringController::on_init() // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); - auto log = get_node()->get_logger(); - RCLCPP_WARN(log, "Parameters read correctly."); + RCLCPP_WARN(get_node()->get_logger(), "Parameters read correctly."); } catch (const std::exception & e) @@ -65,7 +59,7 @@ InterfaceConfiguration FourSteeringController::command_interface_configuration() } for (const auto & joint_name : params_.steer_names) { - conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY); + conf_names.push_back(joint_name + "/" + HW_IF_POSITION); } RCLCPP_WARN(get_node()->get_logger(), "Command interfaces set."); @@ -78,13 +72,13 @@ InterfaceConfiguration FourSteeringController::state_interface_configuration() c std::vector conf_joint_names; for (const auto & joint_name : params_.wheel_names) { - conf_joint_names.push_back(joint_name + "/" + feedback_type()); + conf_joint_names.push_back(joint_name + "/" + HW_IF_VELOCITY); } for (const auto & joint_name : params_.steer_names) { - conf_joint_names.push_back(joint_name + "/" + feedback_type()); + conf_joint_names.push_back(joint_name + "/" + HW_IF_POSITION); } - RCLCPP_WARN(get_node()->get_logger(), "State interfaces set."); + RCLCPP_WARN(get_node()->get_logger(), "State interfaces set."); return {interface_configuration_type::INDIVIDUAL, conf_joint_names}; } @@ -103,7 +97,9 @@ controller_interface::return_type FourSteeringController::update( } return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; + //std::shared_ptr last_command_msg; + + std::shared_ptr last_command_msg; received_velocity_msg_ptr_.get(last_command_msg); if (last_command_msg == nullptr) @@ -112,67 +108,241 @@ controller_interface::return_type FourSteeringController::update( return controller_interface::return_type::ERROR; } - const auto age_of_last_command = time - last_command_msg->header.stamp; - // Brake if cmd_vel has timeout, override the stored command + const auto age_of_last_command = time - last_command_msg->header.stamp; if (age_of_last_command > cmd_vel_timeout_) { - last_command_msg->twist.linear.x = 0.0; - last_command_msg->twist.angular.z = 0.0; + last_command_msg->data.speed = 0.0; + last_command_msg->data.front_steering_angle = 0.0; + last_command_msg->data.rear_steering_angle = 0.0; + last_command_msg->data.acceleration = 0.0; } - Twist command = *last_command_msg; - double & linear_cmd = command.twist.linear.x; + // Save twist command: +/* Twist command = *last_command_msg; + double & linear_cmd_x = command.twist.linear.x; + double & linear_cmd_y = command.twist.linear.y; double & angular_cmd = command.twist.angular.z; - - // check if OpenLoop condition is met. - RCLCPP_WARN(get_node()->get_logger(), "Twist Correctly stored."); + */ + four_wheel_steering_msgs::msg::FourWheelSteeringStamped curr_cmd_4ws = *last_command_msg; const double steering_track = params_.track_ - 2 * params_.wheel_steering_y_offset_; - RCLCPP_WARN(get_node()->get_logger(), "Steering track value: %f", steering_track); - - const double joint_number = params_.wheel_names.size(); - double i = -1.0; - // Compute wheels velocities: - if(fabs(linear_cmd) > 0.001) + //const double sign = copysign(1.0, linear_cmd_x); +/* + if(fabs(linear_cmd_x) > 0.001) { const double vel_steering_offset = (angular_cmd * params_.wheel_steering_y_offset_)/params_.wheel_radius_; - const double sign = copysign(1.0, linear_cmd); - RCLCPP_WARN(get_node()->get_logger(), "vel_steering_offset: %f", vel_steering_offset); - - for(size_t wheel=0; wheel < joint_number; wheel++) - { //front_l - front_r - rear_l - rear_r - wheel_vel_cmd[wheel] = sign * std::hypot((linear_cmd + (i) * angular_cmd*steering_track/2), - (params_.wheel_base_*angular_cmd/2.0)) / params_.wheel_radius_ + (i) * vel_steering_offset; - i *= -1.0; - RCLCPP_WARN(get_node()->get_logger(), "Wheel_cmd[%d]: %f", i,wheel_vel_cmd[wheel]); - } + vel_left_front = sign * std::hypot((linear_cmd_x - angular_cmd * steering_track/2), + (params_.wheel_base_*angular_cmd/2.0)) / params_.wheel_radius_ + - vel_steering_offset; + wheel_vel_cmd[0] = vel_left_front; + + vel_right_front = sign * std::hypot((linear_cmd_x + angular_cmd * steering_track/2), + (params_.wheel_base_*angular_cmd/2.0)) / params_.wheel_radius_ + + vel_steering_offset; + wheel_vel_cmd[1] = vel_right_front; + + vel_left_rear = sign * std::hypot((linear_cmd_x - angular_cmd * steering_track/2), + (params_.wheel_base_*angular_cmd/2.0)) / params_.wheel_radius_ + - vel_steering_offset; + wheel_vel_cmd[2] = vel_left_rear; + + vel_right_rear = sign * std::hypot((linear_cmd_x + angular_cmd * steering_track/2), + (params_.wheel_base_*angular_cmd/2.0)) / params_.wheel_radius_ + + vel_steering_offset; + wheel_vel_cmd[3] = vel_right_rear; + } - // Compute steering angles - if(fabs(2.0*linear_cmd) > fabs(angular_cmd*steering_track)) + //if(fabs(linear_cmd_x) > 0.001 && fabs(angular_cmd) > 0.001 ) + //if(fabs(2.0*linear_cmd_x) > fabs(angular_cmd*steering_track) && fabs(linear_cmd_y) < 0.001) + //if(fabs(linear_cmd_x) > 0.001 && fabs(angular_cmd) > 0.001) + if(fabs(linear_cmd_x) > 0.001) { - for(size_t steer=0; steer < joint_number; steer++) { - // front_l - rear_l - front_r - rear_r - steer_vel_cmd[steer] = (-i) * atan(angular_cmd*params_.wheel_base_ / - (2.0 * linear_cmd + (i) * angular_cmd * steering_track)); - i *= -1.0; + if(fabs(angular_cmd) > 2*fabs(linear_cmd_x)*steering_track) + { + float sign = copysign(1, angular_cmd); + angular_cmd = sign * 2*fabs(linear_cmd_x)*steering_track; } + + front_left_steering = atan(angular_cmd * params_.wheel_base_ / + (2.0 * linear_cmd_x - angular_cmd * steering_track)); + steer_cmd[0] = front_left_steering; + + front_right_steering = atan(angular_cmd * params_.wheel_base_ / + (2.0 * linear_cmd_x + angular_cmd * steering_track)); + steer_cmd[1] = front_right_steering; + + rear_left_steering = -front_left_steering; + rear_right_steering = -front_right_steering; + steer_cmd[2] = rear_left_steering; + steer_cmd[3] = rear_right_steering; + } + else if(fabs(linear_cmd_x) > 0.001) //Straight motion + { + front_left_steering = copysign(M_PI_2, angular_cmd); + steer_cmd[0] = front_left_steering; + + front_right_steering = copysign(M_PI_2, angular_cmd); + steer_cmd[1] = front_right_steering; + + rear_left_steering = -front_left_steering; + rear_right_steering = -front_right_steering; + steer_cmd[2] = rear_left_steering; + steer_cmd[3] = rear_right_steering; + } + + + // In-Phase steering: + if(fabs(linear_cmd_x) > 0.001 && fabs(linear_cmd_y) >= 0.001 && fabs(angular_cmd) < 0.001) + { + wheel_vel = sign * std::hypot(linear_cmd_x, linear_cmd_y); + wheel_vel_cmd[0] = wheel_vel; + wheel_vel_cmd[1] = wheel_vel; + wheel_vel_cmd[2] = wheel_vel; + wheel_vel_cmd[3] = wheel_vel; + + front_left_steering = atan(linear_cmd_y / linear_cmd_x); + steer_cmd[0] = front_left_steering; + front_right_steering = atan(linear_cmd_y / linear_cmd_x); + steer_cmd[1] = front_right_steering; + rear_left_steering = front_left_steering; + steer_cmd[2] = rear_left_steering; + rear_right_steering = front_right_steering; + steer_cmd[3] = rear_right_steering; + // FIXME missing wheel speed + } + else if(fabs(linear_cmd_x) < 0.001 && fabs(linear_cmd_y) < 0.001 && fabs(angular_cmd) > 0.001) + { + // Pivot Steering: + front_left_steering = -atan(params_.wheel_base_ / steering_track); + steer_cmd[0] = front_left_steering; + front_right_steering = atan(params_.wheel_base_ / steering_track); + steer_cmd[1] = front_right_steering; + rear_left_steering = atan(params_.wheel_base_ / steering_track); + steer_cmd[2] = rear_left_steering; + rear_right_steering = -atan(params_.wheel_base_ / steering_track); + steer_cmd[3] = rear_right_steering; + vel_left_front = -angular_cmd; + vel_right_front = angular_cmd; + wheel_vel_cmd[0] = vel_left_front; + wheel_vel_cmd[1] = vel_right_front; + wheel_vel_cmd[2] = -vel_left_front; + wheel_vel_cmd[3] = -vel_right_front; } + for(int index=0; index < joint_number; index++){ + alpha_read[index] = registered_steer_handles_[index].feedback_pos.get().get_value()*180.0/M_PI; + std::cout << "Joint: " << index << " " << "Angle: " << alpha_read[index] << std::endl; + } + std::cout << "===============" << std::endl; + // Set wheels velocities: for (size_t index=0; index < joint_number; index++) { registered_wheel_handles_[index].velocity.get().set_value(wheel_vel_cmd[index]); } - // Set steers velocities: + + for(int index=0; index < joint_number; index++){ + w_speed[index] = registered_wheel_handles_[index].feedback.get().get_value(); + std::cout << "Wheel: " << index << " " << "Speed: " << w_speed[index] << std::endl; + } + std::cout << "===============" << std::endl; + */ + + curr_cmd_4ws.data.front_steering_angle = std::clamp(curr_cmd_4ws.data.front_steering_angle, static_cast(-M_PI_2/2), static_cast(M_PI_2/2)); + curr_cmd_4ws.data.rear_steering_angle = std::clamp(curr_cmd_4ws.data.rear_steering_angle, static_cast(-M_PI_2/2), static_cast(M_PI_2/2)); + + // Compute steering angles + const double tan_front_steering = tan(curr_cmd_4ws.data.front_steering_angle); + const double tan_rear_steering = tan(curr_cmd_4ws.data.rear_steering_angle); + const double steering_diff = steering_track*(tan_front_steering - tan_rear_steering)/2.0; + if(fabs(params_.wheel_base_ - fabs(steering_diff)) > 0.001) + { + front_left_steering = atan(params_.wheel_base_*tan_front_steering/(params_.wheel_base_-steering_diff)); + steer_cmd[0] = front_left_steering; + front_right_steering = atan(params_.wheel_base_*tan_front_steering/(params_.wheel_base_+steering_diff)); + steer_cmd[1] = front_right_steering; + rear_left_steering = atan(params_.wheel_base_*tan_rear_steering/(params_.wheel_base_-steering_diff)); + steer_cmd[2] = rear_left_steering; + rear_right_steering = atan(params_.wheel_base_*tan_rear_steering/(params_.wheel_base_+steering_diff)); + steer_cmd[3] = rear_right_steering; + } + if(tan_front_steering == 1.0) + { + pivot = true; + front_left_steering = -atan(params_.wheel_base_ / steering_track); + steer_cmd[0] = front_left_steering; + front_right_steering = atan(params_.wheel_base_ / steering_track); + steer_cmd[1] = front_right_steering; + rear_left_steering = atan(params_.wheel_base_ / steering_track); + steer_cmd[2] = rear_left_steering; + rear_right_steering = -atan(params_.wheel_base_ / steering_track); + steer_cmd[3] = rear_right_steering; + } + // Compute wheels velocities: + if(fabs(curr_cmd_4ws.data.speed) > 0.001 && !pivot) + { + //Virutal front and rear wheelbase + // distance between the projection of the CIR on the wheelbase and the front axle + double l_front = 0; + if(fabs(tan(front_left_steering) - tan(front_right_steering)) > 0.01) + { + l_front = tan(front_right_steering) * tan(front_left_steering) * steering_track + / (tan(front_left_steering) - tan(front_right_steering)); + } + // distance between the projection of the CIR on the wheelbase and the rear axle + double l_rear = 0; + if(fabs(tan(rear_left_steering) - tan(rear_right_steering)) > 0.01) + { + l_rear = tan(rear_right_steering) * tan(rear_left_steering) * steering_track + / (tan(rear_left_steering) - tan(rear_right_steering)); + } + + const double angular_speed_cmd = curr_cmd_4ws.data.speed * (tan_front_steering-tan_rear_steering)/params_.wheel_base_; + const double vel_steering_offset = (angular_speed_cmd*params_.wheel_steering_y_offset_)/params_.wheel_radius_; + const double sign = copysign(1.0, curr_cmd_4ws.data.speed); + + vel_left_front = sign * std::hypot((curr_cmd_4ws.data.speed - angular_speed_cmd*steering_track/2), + (l_front*angular_speed_cmd))/params_.wheel_radius_ + - vel_steering_offset; + wheel_vel_cmd[0] = vel_left_front; + vel_right_front = sign * std::hypot((curr_cmd_4ws.data.speed + angular_speed_cmd*steering_track/2), + (l_front*angular_speed_cmd))/params_.wheel_radius_ + + vel_steering_offset; + wheel_vel_cmd[1] = vel_right_front; + vel_left_rear = sign * std::hypot((curr_cmd_4ws.data.speed - angular_speed_cmd*steering_track/2), + (l_rear*angular_speed_cmd))/params_.wheel_radius_ + - vel_steering_offset; + wheel_vel_cmd[2] = vel_left_rear; + vel_right_rear = sign * std::hypot((curr_cmd_4ws.data.speed + angular_speed_cmd*steering_track/2), + (l_rear*angular_speed_cmd))/params_.wheel_radius_ + + vel_steering_offset; + wheel_vel_cmd[3] = vel_right_rear; + } + if(pivot) + { + vel_left_front = -curr_cmd_4ws.data.speed; + vel_right_front = curr_cmd_4ws.data.speed; + wheel_vel_cmd[0] = vel_left_front; + wheel_vel_cmd[1] = vel_right_front; + wheel_vel_cmd[2] = vel_left_front; + wheel_vel_cmd[3] = vel_right_front; + pivot = false; + } + //Send steering cmds: for (size_t index=0; index < joint_number; index++) { - registered_steer_handles_[index].velocity.get().set_value(steer_vel_cmd[index]); + registered_steer_handles_[index].position.get().set_value(steer_cmd[index]); + } + //Send speed cmds: + for (size_t index=0; index < joint_number; index++) + { + registered_wheel_handles_[index].velocity.get().set_value(wheel_vel_cmd[index]); } - return controller_interface::return_type::OK; + return controller_interface::return_type::OK; } controller_interface::CallbackReturn FourSteeringController::on_configure( @@ -194,43 +364,38 @@ controller_interface::CallbackReturn FourSteeringController::on_configure( return controller_interface::CallbackReturn::ERROR; } - const double wheel_separation = params_.track_; - const double wheel_radius = params_.wheel_radius_; - const double wheel_steering_y_offset_ = params_.wheel_steering_y_offset_; - if (!reset()) { return controller_interface::CallbackReturn::ERROR; } - const Twist empty_twist; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); - RCLCPP_WARN(get_node()->get_logger(), "Empty Twist Command created."); + const four_wheel_steering_msgs::msg::FourWheelSteeringStamped empty_cmd; + received_velocity_msg_ptr_.set(std::make_shared(empty_cmd)); // Fill last two commands with default constructed commands - previous_commands_.emplace(empty_twist); - previous_commands_.emplace(empty_twist); + previous_commands_.emplace(empty_cmd); + previous_commands_.emplace(empty_cmd); // initialize command subscriber velocity_command_unstamped_subscriber_ = - get_node()->create_subscription( - DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { - RCLCPP_WARN(get_node()->get_logger(), "Entered the lambda function."); - if (!subscriber_is_active_) { RCLCPP_WARN( get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); return; } - // Write fake header in the stored stamped command - std::shared_ptr twist_stamped; - received_velocity_msg_ptr_.get(twist_stamped); - twist_stamped->twist = *msg; - twist_stamped->header.stamp = get_node()->get_clock()->now(); - std::cout << "Msg created all good." << std::endl; - + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.set(std::move(msg)); } ); @@ -247,7 +412,7 @@ controller_interface::CallbackReturn FourSteeringController::on_activate(const r configure_wheels(params_.wheel_names, registered_wheel_handles_); const auto steers_result = configure_steers(params_.steer_names, registered_steer_handles_); - + RCLCPP_WARN(get_node()->get_logger(),"Configuration completed."); if ( wheels_result == controller_interface::CallbackReturn::ERROR || steers_result == controller_interface::CallbackReturn::ERROR) @@ -285,7 +450,7 @@ controller_interface::CallbackReturn FourSteeringController::on_cleanup( return controller_interface::CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); + received_velocity_msg_ptr_.set(std::make_shared()); return controller_interface::CallbackReturn::SUCCESS; } @@ -300,9 +465,8 @@ controller_interface::CallbackReturn FourSteeringController::on_error(const rclc bool FourSteeringController::reset() { - // release the old queue - std::queue empty; + std::queue empty; std::swap(previous_commands_, empty); registered_wheel_handles_.clear(); registered_steer_handles_.clear(); @@ -330,22 +494,32 @@ void FourSteeringController::halt() for (const auto & wheel_handle : wheel_handles) { wheel_handle.velocity.get().set_value(0.0); + + } + }; + + const auto halt_steers = [](auto & steer_handles) + { + for (const auto & steer_handle : steer_handles) + { + steer_handle.position.get().set_value(0.0); } }; halt_wheels(registered_wheel_handles_); - + halt_steers(registered_steer_handles_); + } controller_interface::CallbackReturn FourSteeringController::configure_wheels( const std::vector & wheel_names, std::vector & registered_handles) { - auto logger = get_node()->get_logger(); + RCLCPP_INFO(get_node()->get_logger(), "Get Wheel Joint Instance"); if (wheel_names.empty()) { - RCLCPP_ERROR(logger, "No wheel names specified"); + RCLCPP_ERROR(get_node()->get_logger(), "No wheel names specified"); return controller_interface::CallbackReturn::ERROR; } @@ -353,18 +527,18 @@ controller_interface::CallbackReturn FourSteeringController::configure_wheels( registered_handles.reserve(wheel_names.size()); for (const auto & wheel_name : wheel_names) { - const auto interface_name = feedback_type(); + //const auto interface_name = feedback_type(); const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&wheel_name, &interface_name](const auto & interface) + [&wheel_name](const auto & interface) { return interface.get_prefix_name() == wheel_name && - interface.get_interface_name() == interface_name; + interface.get_interface_name() == HW_IF_VELOCITY; }); if (state_handle == state_interfaces_.cend()) { - RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint state handle for %s", wheel_name.c_str()); return controller_interface::CallbackReturn::ERROR; } @@ -378,7 +552,7 @@ controller_interface::CallbackReturn FourSteeringController::configure_wheels( if (command_handle == command_interfaces_.end()) { - RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", wheel_name.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint command handle for %s", wheel_name.c_str()); return controller_interface::CallbackReturn::ERROR; } @@ -394,11 +568,11 @@ controller_interface::CallbackReturn FourSteeringController::configure_steers( const std::vector & steer_names, std::vector & registered_handles) { - auto logger = get_node()->get_logger(); + RCLCPP_INFO(get_node()->get_logger(), "Get Steering Joint Instance"); if (steer_names.empty()) { - RCLCPP_ERROR(logger, "No steer names specified"); + RCLCPP_ERROR(get_node()->get_logger(), "No steer names specified"); return controller_interface::CallbackReturn::ERROR; } @@ -406,18 +580,18 @@ controller_interface::CallbackReturn FourSteeringController::configure_steers( registered_handles.reserve(steer_names.size()); for (const auto & steer_name : steer_names) { - const auto interface_name = feedback_type(); + //const auto interface_name = feedback_type(); const auto state_handle = std::find_if( state_interfaces_.cbegin(), state_interfaces_.cend(), - [&steer_name, &interface_name](const auto & interface) + [&steer_name](const auto & interface) { return interface.get_prefix_name() == steer_name && - interface.get_interface_name() == interface_name; + interface.get_interface_name() == HW_IF_POSITION; }); if (state_handle == state_interfaces_.cend()) { - RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", steer_name.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint state handle for %s", steer_name.c_str()); return controller_interface::CallbackReturn::ERROR; } @@ -426,12 +600,12 @@ controller_interface::CallbackReturn FourSteeringController::configure_steers( [&steer_name](const auto & interface) { return interface.get_prefix_name() == steer_name && - interface.get_interface_name() == HW_IF_VELOCITY; + interface.get_interface_name() == HW_IF_POSITION; }); if (command_handle == command_interfaces_.end()) { - RCLCPP_ERROR(logger, "Unable to obtain joint command handle for %s", steer_name.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint command handle for %s", steer_name.c_str()); return controller_interface::CallbackReturn::ERROR; } From 85e8c4d6c9f66c1db0faaa2c98a1ba8194c8f7ff Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Wed, 8 Feb 2023 19:02:00 +0100 Subject: [PATCH 09/21] Added Odometry. --- .../four_steering_controller.hpp | 7 + .../src/four_steering_controller.cpp | 208 ++++++++---------- .../four_steering_controller_parameter.yaml | 47 +++- 3 files changed, 146 insertions(+), 116 deletions(-) diff --git a/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp b/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp index 2410f41553..2efd82b680 100644 --- a/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp +++ b/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp @@ -154,12 +154,19 @@ class FourSteeringController : public controller_interface::ControllerInterface double steer_cmd[4] = { 0 }; double alpha_read[4] = { 0 }; double w_speed[4] = { 0 }; + + double ws_read[4] = { 0 }; + double alphas_read[4] = { 0 }; + + bool pivot = false; + double vel_left_front = 0.0, vel_right_front = 0.0; double vel_left_rear = 0.0, vel_right_rear = 0.0; double front_left_steering = 0.0, front_right_steering = 0.0; double rear_left_steering = 0.0, rear_right_steering = 0.0; double wheel_vel = 0.0; + bool reset(); void halt(); }; diff --git a/four_steering_controller/src/four_steering_controller.cpp b/four_steering_controller/src/four_steering_controller.cpp index 0f7eb8de16..357c12817d 100644 --- a/four_steering_controller/src/four_steering_controller.cpp +++ b/four_steering_controller/src/four_steering_controller.cpp @@ -97,7 +97,6 @@ controller_interface::return_type FourSteeringController::update( } return controller_interface::return_type::OK; } - //std::shared_ptr last_command_msg; std::shared_ptr last_command_msg; received_velocity_msg_ptr_.get(last_command_msg); @@ -117,139 +116,77 @@ controller_interface::return_type FourSteeringController::update( last_command_msg->data.rear_steering_angle = 0.0; last_command_msg->data.acceleration = 0.0; } - // Save twist command: -/* Twist command = *last_command_msg; - double & linear_cmd_x = command.twist.linear.x; - double & linear_cmd_y = command.twist.linear.y; - double & angular_cmd = command.twist.angular.z; - */ four_wheel_steering_msgs::msg::FourWheelSteeringStamped curr_cmd_4ws = *last_command_msg; const double steering_track = params_.track_ - 2 * params_.wheel_steering_y_offset_; const double joint_number = params_.wheel_names.size(); - //const double sign = copysign(1.0, linear_cmd_x); -/* - if(fabs(linear_cmd_x) > 0.001) - { - const double vel_steering_offset = (angular_cmd * params_.wheel_steering_y_offset_)/params_.wheel_radius_; - - vel_left_front = sign * std::hypot((linear_cmd_x - angular_cmd * steering_track/2), - (params_.wheel_base_*angular_cmd/2.0)) / params_.wheel_radius_ - - vel_steering_offset; - wheel_vel_cmd[0] = vel_left_front; - vel_right_front = sign * std::hypot((linear_cmd_x + angular_cmd * steering_track/2), - (params_.wheel_base_*angular_cmd/2.0)) / params_.wheel_radius_ - + vel_steering_offset; - wheel_vel_cmd[1] = vel_right_front; - - vel_left_rear = sign * std::hypot((linear_cmd_x - angular_cmd * steering_track/2), - (params_.wheel_base_*angular_cmd/2.0)) / params_.wheel_radius_ - - vel_steering_offset; - wheel_vel_cmd[2] = vel_left_rear; + double steer_radius = hypot((params_.wheel_base_ / tan(curr_cmd_4ws.data.front_steering_angle)) + steering_track/2, params_.wheel_base_); + double angular_speed = curr_cmd_4ws.data.speed / steer_radius; - vel_right_rear = sign * std::hypot((linear_cmd_x + angular_cmd * steering_track/2), - (params_.wheel_base_*angular_cmd/2.0)) / params_.wheel_radius_ - + vel_steering_offset; - wheel_vel_cmd[3] = vel_right_rear; - - } - // Compute steering angles - //if(fabs(linear_cmd_x) > 0.001 && fabs(angular_cmd) > 0.001 ) - //if(fabs(2.0*linear_cmd_x) > fabs(angular_cmd*steering_track) && fabs(linear_cmd_y) < 0.001) - //if(fabs(linear_cmd_x) > 0.001 && fabs(angular_cmd) > 0.001) - if(fabs(linear_cmd_x) > 0.001) + //Read Values from the Handles: + for (size_t index=0; index < joint_number; index++) { - if(fabs(angular_cmd) > 2*fabs(linear_cmd_x)*steering_track) - { - float sign = copysign(1, angular_cmd); - angular_cmd = sign * 2*fabs(linear_cmd_x)*steering_track; - } - - front_left_steering = atan(angular_cmd * params_.wheel_base_ / - (2.0 * linear_cmd_x - angular_cmd * steering_track)); - steer_cmd[0] = front_left_steering; - - front_right_steering = atan(angular_cmd * params_.wheel_base_ / - (2.0 * linear_cmd_x + angular_cmd * steering_track)); - steer_cmd[1] = front_right_steering; + alphas_read[index] = registered_steer_handles_[index].feedback_pos.get().get_value(); + ws_read[index] = registered_wheel_handles_[index].feedback.get().get_value(); + } - rear_left_steering = -front_left_steering; - rear_right_steering = -front_right_steering; - steer_cmd[2] = rear_left_steering; - steer_cmd[3] = rear_right_steering; + if (params_.open_loop) + { + odometry_.updateOpenLoop(curr_cmd_4ws.data.speed, angular_speed, time); } - else if(fabs(linear_cmd_x) > 0.001) //Straight motion + else { - front_left_steering = copysign(M_PI_2, angular_cmd); - steer_cmd[0] = front_left_steering; + double left_feedback_mean = 0.0; + double right_feedback_mean = 0.0; - front_right_steering = copysign(M_PI_2, angular_cmd); - steer_cmd[1] = front_right_steering; + left_feedback_mean = alphas_read[0] + alphas_read[2]; + right_feedback_mean = alphas_read[1] + alphas_read[3]; - rear_left_steering = -front_left_steering; - rear_right_steering = -front_right_steering; - steer_cmd[2] = rear_left_steering; - steer_cmd[3] = rear_right_steering; + left_feedback_mean /= params_.wheels_per_side; + right_feedback_mean /= params_.wheels_per_side; + if (params_.position_feedback) + { + odometry_.update(left_feedback_mean, right_feedback_mean, time); + } + else + { + odometry_.updateFromVelocity( + left_feedback_mean * params_.wheel_radius_ * period.seconds(), + right_feedback_mean * params_.wheel_radius_ * period.seconds(), time); + } } - - // In-Phase steering: - if(fabs(linear_cmd_x) > 0.001 && fabs(linear_cmd_y) >= 0.001 && fabs(angular_cmd) < 0.001) - { - wheel_vel = sign * std::hypot(linear_cmd_x, linear_cmd_y); - wheel_vel_cmd[0] = wheel_vel; - wheel_vel_cmd[1] = wheel_vel; - wheel_vel_cmd[2] = wheel_vel; - wheel_vel_cmd[3] = wheel_vel; + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - front_left_steering = atan(linear_cmd_y / linear_cmd_x); - steer_cmd[0] = front_left_steering; - front_right_steering = atan(linear_cmd_y / linear_cmd_x); - steer_cmd[1] = front_right_steering; - rear_left_steering = front_left_steering; - steer_cmd[2] = rear_left_steering; - rear_right_steering = front_right_steering; - steer_cmd[3] = rear_right_steering; - // FIXME missing wheel speed - } - else if(fabs(linear_cmd_x) < 0.001 && fabs(linear_cmd_y) < 0.001 && fabs(angular_cmd) > 0.001) + if (realtime_odometry_publisher_->trylock()) { - // Pivot Steering: - front_left_steering = -atan(params_.wheel_base_ / steering_track); - steer_cmd[0] = front_left_steering; - front_right_steering = atan(params_.wheel_base_ / steering_track); - steer_cmd[1] = front_right_steering; - rear_left_steering = atan(params_.wheel_base_ / steering_track); - steer_cmd[2] = rear_left_steering; - rear_right_steering = -atan(params_.wheel_base_ / steering_track); - steer_cmd[3] = rear_right_steering; - vel_left_front = -angular_cmd; - vel_right_front = angular_cmd; - wheel_vel_cmd[0] = vel_left_front; - wheel_vel_cmd[1] = vel_right_front; - wheel_vel_cmd[2] = -vel_left_front; - wheel_vel_cmd[3] = -vel_right_front; - } - - for(int index=0; index < joint_number; index++){ - alpha_read[index] = registered_steer_handles_[index].feedback_pos.get().get_value()*180.0/M_PI; - std::cout << "Joint: " << index << " " << "Angle: " << alpha_read[index] << std::endl; + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = time; + odometry_message.pose.pose.position.x = odometry_.getX(); + odometry_message.pose.pose.position.y = odometry_.getY(); + odometry_message.pose.pose.orientation.x = orientation.x(); + odometry_message.pose.pose.orientation.y = orientation.y(); + odometry_message.pose.pose.orientation.z = orientation.z(); + odometry_message.pose.pose.orientation.w = orientation.w(); + odometry_message.twist.twist.linear.x = odometry_.getLinear(); + odometry_message.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->unlockAndPublish(); } - std::cout << "===============" << std::endl; - // Set wheels velocities: - for (size_t index=0; index < joint_number; index++) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { - registered_wheel_handles_[index].velocity.get().set_value(wheel_vel_cmd[index]); - } - - for(int index=0; index < joint_number; index++){ - w_speed[index] = registered_wheel_handles_[index].feedback.get().get_value(); - std::cout << "Wheel: " << index << " " << "Speed: " << w_speed[index] << std::endl; + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->unlockAndPublish(); } - std::cout << "===============" << std::endl; - */ curr_cmd_4ws.data.front_steering_angle = std::clamp(curr_cmd_4ws.data.front_steering_angle, static_cast(-M_PI_2/2), static_cast(M_PI_2/2)); curr_cmd_4ws.data.rear_steering_angle = std::clamp(curr_cmd_4ws.data.rear_steering_angle, static_cast(-M_PI_2/2), static_cast(M_PI_2/2)); @@ -368,6 +305,8 @@ controller_interface::CallbackReturn FourSteeringController::on_configure( { return controller_interface::CallbackReturn::ERROR; } + odometry_.setWheelParams(params_.track_, params_.wheel_radius_); + odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); const four_wheel_steering_msgs::msg::FourWheelSteeringStamped empty_cmd; received_velocity_msg_ptr_.set(std::make_shared(empty_cmd)); @@ -399,7 +338,46 @@ controller_interface::CallbackReturn FourSteeringController::on_configure( } ); - previous_update_timestamp_ = get_node()->get_clock()->now(); + + // initialize odometry publisher and messasge + odometry_publisher_ = get_node()->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_publisher_ = + std::make_shared>( + odometry_publisher_); + + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.frame_id = params_.odom_frame_id; + odometry_message.child_frame_id = params_.base_frame_id; + + // initialize odom values zeros + odometry_message.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); + + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + } + + if (params_.enable_odom_tf) + { + // initialize transform publisher and message + odometry_transform_publisher_ = get_node()->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_transform_publisher_ = + std::make_shared>( + odometry_transform_publisher_); + + // keeping track of odom and base_link transforms only + auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; + odometry_transform_message.transforms.resize(1); + odometry_transform_message.transforms.front().header.frame_id = params_.odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = params_.base_frame_id; + } return CallbackReturn::SUCCESS; } diff --git a/four_steering_controller/src/four_steering_controller_parameter.yaml b/four_steering_controller/src/four_steering_controller_parameter.yaml index 5b77798b39..f81f6899b2 100644 --- a/four_steering_controller/src/four_steering_controller_parameter.yaml +++ b/four_steering_controller/src/four_steering_controller_parameter.yaml @@ -36,11 +36,56 @@ four_steering_controller: } cmd_vel_timeout: { type: double, - default_value: 4.0, # Hz + default_value: 0.5, # Seconds description: "Timeout on cmd_vel command.", } publish_rate: { type: double, default_value: 50.0, # Hz description: "Publishing rate (Hz) of the odometry and TF messages.", + } + wheels_per_side: { + type: int, + default_value: 2, + description: "Number of wheels on each wide of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number or control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", + } + open_loop: { + type: bool, + default_value: false, + description: "If set to false the odometry of the robot will be calculated from the commanded values and not from feedback.", + } + odom_frame_id: { + type: string, + default_value: "odom", + description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", + } + base_frame_id: { + type: string, + default_value: "base_link", + description: "Name of the robot's base frame that is child of the odometry frame.", + } + pose_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + twist_covariance_diagonal: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + } + enable_odom_tf: { + type: bool, + default_value: true, + description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + } + position_feedback: { + type: bool, + default_value: true, + description: "Is there position feedback from hardware.", + } + velocity_rolling_window_size: { + type: int, + default_value: 10, + description: "Size of the rolling window for calculation of mean velocity use in odometry.", } \ No newline at end of file From 665d4defe5945290ecfc86043835133797e9b94b Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Sat, 11 Feb 2023 18:40:36 +0100 Subject: [PATCH 10/21] Code refactoring, added unstamped msg support. --- four_steering_controller/CMakeLists.txt | 1 + .../four_steering_controller.hpp | 29 ++-- .../four_steering_controller/odometry.hpp | 22 ++- four_steering_controller/package.xml | 1 + .../src/four_steering_controller.cpp | 136 +++++++++++------- .../four_steering_controller_parameter.yaml | 5 + four_steering_controller/src/odometry.cpp | 60 +++++--- 7 files changed, 157 insertions(+), 97 deletions(-) diff --git a/four_steering_controller/CMakeLists.txt b/four_steering_controller/CMakeLists.txt index b4d4486b27..ae00a42357 100644 --- a/four_steering_controller/CMakeLists.txt +++ b/four_steering_controller/CMakeLists.txt @@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs hardware_interface nav_msgs + std_srvs pluginlib rclcpp rclcpp_lifecycle diff --git a/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp b/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp index 2efd82b680..b521174f6d 100644 --- a/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp +++ b/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp @@ -22,10 +22,12 @@ #include "realtime_tools/realtime_box.h" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/empty.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "four_steering_controller_parameters.hpp" #include "four_wheel_steering_msgs/msg/four_wheel_steering_stamped.hpp" +#include "four_wheel_steering_msgs/msg/four_wheel_steering.hpp" namespace four_steering_controller { @@ -123,23 +125,14 @@ class FourSteeringController : public controller_interface::ControllerInterface realtime_odometry_transform_publisher_ = nullptr; bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - rclcpp::Subscription::SharedPtr - velocity_command_unstamped_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr velocity_command_unstamped_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr velocity_command_stamped_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; - std::queue previous_commands_; // last two commands - - // speed limiters - SpeedLimiter limiter_linear_; - SpeedLimiter limiter_angular_; - - bool publish_limited_velocity_ = false; - std::shared_ptr> limited_velocity_publisher_ = nullptr; - std::shared_ptr> realtime_limited_velocity_publisher_ = - nullptr; + rclcpp::Service::SharedPtr reset_odom_service_; + std::queue previous_commands_; // last two commands rclcpp::Time previous_update_timestamp_{0}; // publish rate limiter @@ -152,21 +145,19 @@ class FourSteeringController : public controller_interface::ControllerInterface double wheel_vel_cmd[4] = { 0 }; double steer_cmd[4] = { 0 }; - double alpha_read[4] = { 0 }; - double w_speed[4] = { 0 }; - double ws_read[4] = { 0 }; double alphas_read[4] = { 0 }; - - bool pivot = false; - double vel_left_front = 0.0, vel_right_front = 0.0; double vel_left_rear = 0.0, vel_right_rear = 0.0; double front_left_steering = 0.0, front_right_steering = 0.0; double rear_left_steering = 0.0, rear_right_steering = 0.0; double wheel_vel = 0.0; + void reset_odometry( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res); bool reset(); void halt(); }; diff --git a/four_steering_controller/include/four_steering_controller/odometry.hpp b/four_steering_controller/include/four_steering_controller/odometry.hpp index d04280af5e..234438b859 100644 --- a/four_steering_controller/include/four_steering_controller/odometry.hpp +++ b/four_steering_controller/include/four_steering_controller/odometry.hpp @@ -14,7 +14,9 @@ class Odometry explicit Odometry(size_t velocity_rolling_window_size = 10); void init(const rclcpp::Time & time); - bool update(double left_pos, double right_pos, const rclcpp::Time & time); + bool update(const double &fl_speed, const double &fr_speed, + const double &rl_speed, const double &rr_speed, + double front_steering, double rear_steering, const rclcpp::Time &time); bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time); void updateOpenLoop(double linear, double angular, const rclcpp::Time & time); void resetOdometry(); @@ -23,14 +25,18 @@ class Odometry double getY() const { return y_; } double getHeading() const { return heading_; } double getLinear() const { return linear_; } + double getLinearX() const { return linear_x_; } + double getLinearY() const { return linear_y_; } + double getAngular() const { return angular_; } - void setWheelParams(double wheel_separation, double wheel_radius); + void setWheelParams( double steering_track, double wheel_radius, + double wheel_base, double wheel_steering_y_offset); void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); private: using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; - + void integrateXY(double linear_x, double linear_y, double angular); void integrateRungeKutta2(double linear, double angular); void integrateExact(double linear, double angular); void resetAccumulators(); @@ -46,14 +52,16 @@ class Odometry // Current velocity: double linear_; // [m/s] double angular_; // [rad/s] - + double linear_x_; + double linear_y_; // Wheel kinematic parameters [m]: double wheel_separation_; double wheel_radius_; - + double wheel_steering_y_offset_; + double wheel_base_; + double steering_track_; // Previous wheel position/state [rad]: - double left_wheel_old_pos_; - double right_wheel_old_pos_; + double wheel_old_pos_; // Rolling mean accumulators for the linear and angular velocities: size_t velocity_rolling_window_size_; diff --git a/four_steering_controller/package.xml b/four_steering_controller/package.xml index 64d49d9bf1..1a3e600846 100644 --- a/four_steering_controller/package.xml +++ b/four_steering_controller/package.xml @@ -16,6 +16,7 @@ geometry_msgs hardware_interface nav_msgs + std_srvs rclcpp rclcpp_lifecycle rcpputils diff --git a/four_steering_controller/src/four_steering_controller.cpp b/four_steering_controller/src/four_steering_controller.cpp index 357c12817d..f6907dd067 100644 --- a/four_steering_controller/src/four_steering_controller.cpp +++ b/four_steering_controller/src/four_steering_controller.cpp @@ -17,6 +17,7 @@ constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; +constexpr auto DEFAULT_RESET_ODOM_SERVICE = "~/reset_odometry"; } // namespace namespace four_steering_controller @@ -112,9 +113,8 @@ controller_interface::return_type FourSteeringController::update( if (age_of_last_command > cmd_vel_timeout_) { last_command_msg->data.speed = 0.0; - last_command_msg->data.front_steering_angle = 0.0; - last_command_msg->data.rear_steering_angle = 0.0; last_command_msg->data.acceleration = 0.0; + last_command_msg->data.jerk = 0.0; } four_wheel_steering_msgs::msg::FourWheelSteeringStamped curr_cmd_4ws = *last_command_msg; @@ -147,7 +147,22 @@ controller_interface::return_type FourSteeringController::update( right_feedback_mean /= params_.wheels_per_side; if (params_.position_feedback) { - odometry_.update(left_feedback_mean, right_feedback_mean, time); + double front_steering_pos = 0.0; + if(fabs(alphas_read[0]) > 0.001 || fabs(alphas_read[1]) > 0.001) + { + front_steering_pos = atan(2*tan(alphas_read[0])*tan(alphas_read[1])/ + (tan(alphas_read[0]) + tan(alphas_read[1]))); + } + + double rear_steering_pos = 0.0; + if(fabs(alphas_read[2]) > 0.001 || fabs(alphas_read[3]) > 0.001) + { + rear_steering_pos = atan(2*tan(alphas_read[2])*tan(alphas_read[3])/ + (tan(alphas_read[2]) + tan(alphas_read[3]))); + } + + odometry_.update(ws_read[0], ws_read[1], ws_read[2], ws_read[3], + front_steering_pos, rear_steering_pos, time); } else { @@ -159,7 +174,6 @@ controller_interface::return_type FourSteeringController::update( tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (realtime_odometry_publisher_->trylock()) { auto & odometry_message = realtime_odometry_publisher_->msg_; @@ -170,11 +184,10 @@ controller_interface::return_type FourSteeringController::update( odometry_message.pose.pose.orientation.y = orientation.y(); odometry_message.pose.pose.orientation.z = orientation.z(); odometry_message.pose.pose.orientation.w = orientation.w(); - odometry_message.twist.twist.linear.x = odometry_.getLinear(); + odometry_message.twist.twist.linear.x = odometry_.getLinearX(); odometry_message.twist.twist.angular.z = odometry_.getAngular(); realtime_odometry_publisher_->unlockAndPublish(); } - if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) { auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); @@ -188,8 +201,8 @@ controller_interface::return_type FourSteeringController::update( realtime_odometry_transform_publisher_->unlockAndPublish(); } - curr_cmd_4ws.data.front_steering_angle = std::clamp(curr_cmd_4ws.data.front_steering_angle, static_cast(-M_PI_2/2), static_cast(M_PI_2/2)); - curr_cmd_4ws.data.rear_steering_angle = std::clamp(curr_cmd_4ws.data.rear_steering_angle, static_cast(-M_PI_2/2), static_cast(M_PI_2/2)); + curr_cmd_4ws.data.front_steering_angle = std::clamp(curr_cmd_4ws.data.front_steering_angle, -M_PI_4, M_PI_4); + curr_cmd_4ws.data.rear_steering_angle = std::clamp(curr_cmd_4ws.data.rear_steering_angle, -M_PI_4, M_PI_4); // Compute steering angles const double tan_front_steering = tan(curr_cmd_4ws.data.front_steering_angle); @@ -206,20 +219,9 @@ controller_interface::return_type FourSteeringController::update( rear_right_steering = atan(params_.wheel_base_*tan_rear_steering/(params_.wheel_base_+steering_diff)); steer_cmd[3] = rear_right_steering; } - if(tan_front_steering == 1.0) - { - pivot = true; - front_left_steering = -atan(params_.wheel_base_ / steering_track); - steer_cmd[0] = front_left_steering; - front_right_steering = atan(params_.wheel_base_ / steering_track); - steer_cmd[1] = front_right_steering; - rear_left_steering = atan(params_.wheel_base_ / steering_track); - steer_cmd[2] = rear_left_steering; - rear_right_steering = -atan(params_.wheel_base_ / steering_track); - steer_cmd[3] = rear_right_steering; - } + // Compute wheels velocities: - if(fabs(curr_cmd_4ws.data.speed) > 0.001 && !pivot) + if(fabs(curr_cmd_4ws.data.speed) >= 0.0) { //Virutal front and rear wheelbase // distance between the projection of the CIR on the wheelbase and the front axle @@ -258,16 +260,7 @@ controller_interface::return_type FourSteeringController::update( + vel_steering_offset; wheel_vel_cmd[3] = vel_right_rear; } - if(pivot) - { - vel_left_front = -curr_cmd_4ws.data.speed; - vel_right_front = curr_cmd_4ws.data.speed; - wheel_vel_cmd[0] = vel_left_front; - wheel_vel_cmd[1] = vel_right_front; - wheel_vel_cmd[2] = vel_left_front; - wheel_vel_cmd[3] = vel_right_front; - pivot = false; - } + //Send steering cmds: for (size_t index=0; index < joint_number; index++) { @@ -305,7 +298,7 @@ controller_interface::CallbackReturn FourSteeringController::on_configure( { return controller_interface::CallbackReturn::ERROR; } - odometry_.setWheelParams(params_.track_, params_.wheel_radius_); + odometry_.setWheelParams(params_.track_ - 2*params_.wheel_steering_y_offset_, params_.wheel_radius_, params_.wheel_base_, params_.wheel_steering_y_offset_); odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); const four_wheel_steering_msgs::msg::FourWheelSteeringStamped empty_cmd; @@ -315,10 +308,34 @@ controller_interface::CallbackReturn FourSteeringController::on_configure( previous_commands_.emplace(empty_cmd); // initialize command subscriber - velocity_command_unstamped_subscriber_ = - get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + if(params_.use_stamped_cmd_) + { + velocity_command_stamped_subscriber_ = + get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received FourWheelSteeringStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.set(std::move(msg)); + } + ); + } else { + velocity_command_unstamped_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { @@ -326,18 +343,14 @@ controller_interface::CallbackReturn FourSteeringController::on_configure( get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); return; } - if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) - { - RCLCPP_WARN_ONCE( - get_node()->get_logger(), - "Received TwistStamped with zero timestamp, setting it to current " - "time, this message will only be shown once"); - msg->header.stamp = get_node()->get_clock()->now(); - } - received_velocity_msg_ptr_.set(std::move(msg)); - } - ); + // Write fake header in the stored stamped command + std::shared_ptr four_ws_stamped; + received_velocity_msg_ptr_.get(four_ws_stamped); + four_ws_stamped->data = *msg; + four_ws_stamped->header.stamp = get_node()->get_clock()->now(); + }); + } // initialize odometry publisher and messasge odometry_publisher_ = get_node()->create_publisher( @@ -379,12 +392,18 @@ controller_interface::CallbackReturn FourSteeringController::on_configure( odometry_transform_message.transforms.front().child_frame_id = params_.base_frame_id; } + // Create odom reset service + reset_odom_service_ = get_node()->create_service( + DEFAULT_RESET_ODOM_SERVICE, std::bind( + &FourSteeringController::reset_odometry, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn FourSteeringController::on_activate(const rclcpp_lifecycle::State &) { - std::cout << "On_activate: configuring wheels and steers. " << std::endl; + std::cout << "On_activate: configuring Wheels and Steers. " << std::endl; const auto wheels_result = configure_wheels(params_.wheel_names, registered_wheel_handles_); @@ -441,21 +460,31 @@ controller_interface::CallbackReturn FourSteeringController::on_error(const rclc return controller_interface::CallbackReturn::SUCCESS; } +void FourSteeringController::reset_odometry( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*req*/, + std::shared_ptr /*res*/) +{ + odometry_.resetOdometry(); + RCLCPP_INFO(get_node()->get_logger(), "Odometry successfully reset"); +} + + bool FourSteeringController::reset() { // release the old queue - std::queue empty; - std::swap(previous_commands_, empty); + std::queue empty_four_ws; + std::swap(previous_commands_, empty_four_ws); + registered_wheel_handles_.clear(); registered_steer_handles_.clear(); subscriber_is_active_ = false; - velocity_command_subscriber_.reset(); + velocity_command_stamped_subscriber_.reset(); velocity_command_unstamped_subscriber_.reset(); received_velocity_msg_ptr_.set(nullptr); is_halted = false; - return true; } @@ -472,10 +501,8 @@ void FourSteeringController::halt() for (const auto & wheel_handle : wheel_handles) { wheel_handle.velocity.get().set_value(0.0); - } }; - const auto halt_steers = [](auto & steer_handles) { for (const auto & steer_handle : steer_handles) @@ -486,7 +513,6 @@ void FourSteeringController::halt() halt_wheels(registered_wheel_handles_); halt_steers(registered_steer_handles_); - } controller_interface::CallbackReturn FourSteeringController::configure_wheels( diff --git a/four_steering_controller/src/four_steering_controller_parameter.yaml b/four_steering_controller/src/four_steering_controller_parameter.yaml index f81f6899b2..c553294e5e 100644 --- a/four_steering_controller/src/four_steering_controller_parameter.yaml +++ b/four_steering_controller/src/four_steering_controller_parameter.yaml @@ -88,4 +88,9 @@ four_steering_controller: type: int, default_value: 10, description: "Size of the rolling window for calculation of mean velocity use in odometry.", + } + use_stamped_cmd_: { + type: bool, + default_value: true, + description: "If the command messages are stamped or not.", } \ No newline at end of file diff --git a/four_steering_controller/src/odometry.cpp b/four_steering_controller/src/odometry.cpp index eda3505e65..3855e70849 100644 --- a/four_steering_controller/src/odometry.cpp +++ b/four_steering_controller/src/odometry.cpp @@ -8,11 +8,15 @@ Odometry::Odometry(size_t velocity_rolling_window_size) y_(0.0), heading_(0.0), linear_(0.0), + linear_x_(0.0), + linear_y_(0.0), angular_(0.0), wheel_separation_(0.0), wheel_radius_(0.0), - left_wheel_old_pos_(0.0), - right_wheel_old_pos_(0.0), + wheel_base_(0.0), + wheel_steering_y_offset_(0.0), + steering_track_(0.0), + wheel_old_pos_(0.0), velocity_rolling_window_size_(velocity_rolling_window_size), linear_accumulator_(velocity_rolling_window_size), angular_accumulator_(velocity_rolling_window_size) @@ -26,7 +30,9 @@ void Odometry::init(const rclcpp::Time & time) timestamp_ = time; } -bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & time) +bool Odometry::update(const double &fl_speed, const double &fr_speed, + const double &rl_speed, const double &rr_speed, + double front_steering, double rear_steering, const rclcpp::Time &time) { // We cannot estimate the speed with very small time intervals: const double dt = time.seconds() - timestamp_.seconds(); @@ -35,19 +41,29 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti return false; // Interval too small to integrate with } - // Get current wheel joint positions: - const double left_wheel_cur_pos = left_pos * wheel_radius_; - const double right_wheel_cur_pos = right_pos * wheel_radius_; + const double front_tmp = cos(front_steering)*(tan(front_steering)-tan(rear_steering))/wheel_base_; + const double front_left_tmp = front_tmp/sqrt(1-steering_track_*front_tmp*cos(front_steering) + pow(steering_track_*front_tmp/2,2)); + const double front_right_tmp = front_tmp/sqrt(1+steering_track_*front_tmp*cos(front_steering) + pow(steering_track_*front_tmp/2,2)); + const double fl_speed_tmp = fl_speed * (1/(1-wheel_steering_y_offset_*front_left_tmp)); + const double fr_speed_tmp = fr_speed * (1/(1-wheel_steering_y_offset_*front_right_tmp)); + const double front_linear_speed = wheel_radius_ * copysign(1.0, fl_speed_tmp+fr_speed_tmp) * sqrt((pow(fl_speed,2)+pow(fr_speed,2))/(2+pow(steering_track_*front_tmp,2)/2.0)); - // Estimate velocity of wheels using old and current position: - const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; - const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; + const double rear_tmp = cos(rear_steering)*(tan(front_steering)-tan(rear_steering))/wheel_base_; + const double rear_left_tmp = rear_tmp/sqrt(1-steering_track_*rear_tmp*cos(rear_steering) + pow(steering_track_*rear_tmp/2,2)); + const double rear_right_tmp = rear_tmp/sqrt(1+steering_track_*rear_tmp*cos(rear_steering) + pow(steering_track_*rear_tmp/2,2)); + const double rl_speed_tmp = rl_speed * (1/(1-wheel_steering_y_offset_*rear_left_tmp)); + const double rr_speed_tmp = rr_speed * (1/(1-wheel_steering_y_offset_*rear_right_tmp)); + const double rear_linear_speed = wheel_radius_ * copysign(1.0, rl_speed_tmp+rr_speed_tmp) * sqrt((pow(rl_speed_tmp,2)+pow(rr_speed_tmp,2)) / (2+pow(steering_track_*rear_tmp,2)/2.0)); - // Update old position with current: - left_wheel_old_pos_ = left_wheel_cur_pos; - right_wheel_old_pos_ = right_wheel_cur_pos; + angular_ = (front_linear_speed*front_tmp + rear_linear_speed*rear_tmp)/2.0; - updateFromVelocity(left_wheel_est_vel, right_wheel_est_vel, time); + linear_x_ = (front_linear_speed*cos(front_steering) + rear_linear_speed*cos(rear_steering))/2.0; + linear_y_ = (front_linear_speed*sin(front_steering) - wheel_base_*angular_/2.0 + rear_linear_speed*sin(rear_steering) + wheel_base_*angular_/2.0)/2.0; + linear_ = copysign(1.0, rear_linear_speed)*sqrt(pow(linear_x_,2)+pow(linear_y_,2)); + + timestamp_ = time; + /// Integrate odometry: + integrateXY(linear_x_*dt, linear_y_*dt, angular_*dt); return true; } @@ -95,11 +111,23 @@ void Odometry::resetOdometry() heading_ = 0.0; } +void Odometry::integrateXY(double linear_x, double linear_y, double angular) +{ + const double delta_x = linear_x*cos(heading_) - linear_y*sin(heading_); + const double delta_y = linear_x*sin(heading_) + linear_y*cos(heading_); + + x_ += delta_x; + y_ += delta_y; + heading_ += angular; +} + void Odometry::setWheelParams( - double wheel_separation, double wheel_radius) + double steering_track, double wheel_radius, double wheel_base, double wheel_steering_y_offset) { - wheel_separation_ = wheel_separation; - wheel_radius_ = wheel_radius; + steering_track_ = steering_track; + wheel_steering_y_offset_ = wheel_steering_y_offset; + wheel_radius_ = wheel_radius; + wheel_base_ = wheel_base; } void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) From aff7301c230824b9bb692a1e5187ec3b54281516 Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Sat, 11 Feb 2023 18:42:34 +0100 Subject: [PATCH 11/21] removed old files --- four_steering_controller/src/interval.cpp | 172 ---------------------- 1 file changed, 172 deletions(-) delete mode 100644 four_steering_controller/src/interval.cpp diff --git a/four_steering_controller/src/interval.cpp b/four_steering_controller/src/interval.cpp deleted file mode 100644 index 400001e72c..0000000000 --- a/four_steering_controller/src/interval.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Mark Naeem - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * The names of the contributors may NOT be used to endorse or - * promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Mark Naeem - */ - -#include - -interval::interval(const std::array &limits, const std::string &type) -{ - const auto& types_val = types_dict_[type]; - this->limits_.push_back(limits); - this->types_.push_back(types_val); -} - - -bool interval::intersect(std::array& limits1, const std::array& limits2, std::array& type1, const std::array& type2) -{ - bool interval1_point, interval2_point = false; - if (limits1[0] == limits1[1]) interval1_point = true; - if (limits2[0] == limits2[1]) interval2_point = true; - - if (interval1_point && interval2_point) - return (limits1[0]==limits2[0])? true: false; - - else if (interval1_point && !interval2_point) - { - if (limits1[0]==limits2[0]) - return (type2[0]==0)?false:true; - else if (limits1[0]==limits2[1]) - return (type2[1]==0)?false:true; - else if (limits1[0]>limits2[0] && limits1[0]limits1[0] && limits2[0]limits_.size(); - const auto size2 = another_interval.limits_.size(); - - if (size1==1 && size2==1) - return intersect(limits_[0], another_interval.limits_[0], types_[0], another_interval.types_[0]); - else if (size1==1 && size2==2) - { - const auto& b1 = intersect(limits_[0], another_interval.limits_[0], types_[0], another_interval.types_[0]); - const auto& b2 = intersect(limits_[0], another_interval.limits_[1], types_[0], another_interval.types_[1]); - return b1 || b2; - } - else if (size1==2 && size2==1) - { - const auto& b1 = intersect(limits_[0], another_interval.limits_[0], types_[0], another_interval.types_[0]); - const auto& b2 = intersect(limits_[1], another_interval.limits_[0], types_[1], another_interval.types_[0]); - return b1 || b2; - } - else - { - const auto& b1 = intersect(limits_[0], another_interval.limits_[0], types_[0], another_interval.types_[0]); - const auto& b2 = intersect(limits_[1], another_interval.limits_[0], types_[1], another_interval.types_[0]); - const auto& b3 = intersect(limits_[0], another_interval.limits_[1], types_[0], another_interval.types_[1]); - const auto& b4 = intersect(limits_[1], another_interval.limits_[1], types_[1], another_interval.types_[1]); - return b1 || b2 || b3 || b4; - } -} - -interval interval::complement () const -{ - /*if (limits_.size()>1) //do something because we can't do it yet*/ - interval complementary_interval(std::array{-(M_PI+0.1),limits_[0][0]}, types_[0][0]==1 ? "closeopen" : "close"); - complementary_interval.limits_.push_back(std::array{limits_[0][1],(M_PI+0.1)}); - complementary_interval.types_.push_back(types_[0][1]==1 ? std::array{0,1} : std::array{1,1}); - return complementary_interval; -} - -double interval::len() const -{ - double res = 0; - for (auto it: limits_) // we need this to be a copy not a reference - { - if (it[0]==-(M_PI+0.1)) it[0] =-M_PI; - if (it[1]== (M_PI+0.1)) it[1] = M_PI; - res += fabs(it[0]-it[1]); - } - return res; -} - -void interval::print()const -{ - for (const auto& it: limits_) - { - std::cout<<"[ "< Date: Mon, 13 Feb 2023 10:53:48 +0100 Subject: [PATCH 12/21] Added LimitVel. Support for Twist command. --- .../four_steering_controller.hpp | 20 +- four_steering_controller/package.xml | 5 +- .../src/four_steering_controller.cpp | 306 ++++++++++++------ .../four_steering_controller_parameter.yaml | 5 + 4 files changed, 232 insertions(+), 104 deletions(-) diff --git a/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp b/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp index b521174f6d..93340cb633 100644 --- a/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp +++ b/four_steering_controller/include/four_steering_controller/four_steering_controller.hpp @@ -33,7 +33,6 @@ namespace four_steering_controller { class FourSteeringController : public controller_interface::ControllerInterface { - using Twist = geometry_msgs::msg::TwistStamped; public: FOUR_STEERING_CONTROLLER_PUBLIC @@ -127,22 +126,31 @@ class FourSteeringController : public controller_interface::ControllerInterface bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_unstamped_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr velocity_command_stamped_subscriber_ = nullptr; - + rclcpp::Subscription::SharedPtr velocity_twist_cmd_stamped_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr velocity_twist_cmd_unstamped_subscriber_ = nullptr; + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + realtime_tools::RealtimeBox> received_velocity_twist_msg_ptr_{nullptr}; + rclcpp::Service::SharedPtr reset_odom_service_; std::queue previous_commands_; // last two commands + std::queue previous_twist_commands_; rclcpp::Time previous_update_timestamp_{0}; - // publish rate limiter - double publish_rate_ = 50.0; - rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); - rclcpp::Time previous_publish_timestamp_{0}; + //Limiters + SpeedLimiter limiter_lin_; + SpeedLimiter limiter_ang_; bool is_halted = false; bool use_stamped_vel_ = true; + geometry_msgs::msg::TwistStamped last0_cmd_; + geometry_msgs::msg::TwistStamped last1_cmd_; + four_wheel_steering_msgs::msg::FourWheelSteeringStamped last0_cmd_4ws_; + four_wheel_steering_msgs::msg::FourWheelSteeringStamped last1_cmd_4ws_; + double wheel_vel_cmd[4] = { 0 }; double steer_cmd[4] = { 0 }; double ws_read[4] = { 0 }; diff --git a/four_steering_controller/package.xml b/four_steering_controller/package.xml index 1a3e600846..6a6afdc536 100644 --- a/four_steering_controller/package.xml +++ b/four_steering_controller/package.xml @@ -1,10 +1,9 @@ four_steering_controller - 2.15.0 + 0.0.0 Controller for a 4 steering drive mobile base. - Bence Magyar - Jordan Palacios + Graziato Davide Apache License 2.0 diff --git a/four_steering_controller/src/four_steering_controller.cpp b/four_steering_controller/src/four_steering_controller.cpp index f6907dd067..9bc360abe4 100644 --- a/four_steering_controller/src/four_steering_controller.cpp +++ b/four_steering_controller/src/four_steering_controller.cpp @@ -12,8 +12,10 @@ namespace { -constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; -constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; +constexpr auto DEFAULT_4WSCOMMAND_TOPIC = "~/four_ws_cmd_vel"; +constexpr auto DEFAULT_4WSCOMMAND_UNSTAMPED_TOPIC = "~/four_ws_cmd_vel_unstamped"; +constexpr auto DEFAULT_TWIST_COMMAND_TOPIC = "~/twist_cmd_vel"; +constexpr auto DEFAULT_TWIST_COMMAND_UNSTAMPED_TOPIC = "~/twist_cmd_vel_unstamped"; constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; @@ -99,25 +101,29 @@ controller_interface::return_type FourSteeringController::update( return controller_interface::return_type::OK; } - std::shared_ptr last_command_msg; - received_velocity_msg_ptr_.get(last_command_msg); - - if (last_command_msg == nullptr) + std::shared_ptr last_twist_cmd_msg; + received_velocity_twist_msg_ptr_.get(last_twist_cmd_msg); + const auto age_of_last_twist_command = time - last_twist_cmd_msg->header.stamp; + // Brake if cmd_vel has timeout, override the stored command + if (age_of_last_twist_command > cmd_vel_timeout_) { - RCLCPP_WARN(logger, "Velocity message received was a nullptr."); - return controller_interface::return_type::ERROR; + last_twist_cmd_msg->twist.linear.x = 0.0; + last_twist_cmd_msg->twist.angular.z = 0.0; } + geometry_msgs::msg::TwistStamped curr_cmd_twist = *last_twist_cmd_msg; - // Brake if cmd_vel has timeout, override the stored command + std::shared_ptr last_command_msg; + received_velocity_msg_ptr_.get(last_command_msg); const auto age_of_last_command = time - last_command_msg->header.stamp; + // Brake if cmd_vel has timeout, override the stored command if (age_of_last_command > cmd_vel_timeout_) { last_command_msg->data.speed = 0.0; last_command_msg->data.acceleration = 0.0; last_command_msg->data.jerk = 0.0; } + four_wheel_steering_msgs::msg::FourWheelSteeringStamped curr_cmd_4ws = *last_command_msg; - four_wheel_steering_msgs::msg::FourWheelSteeringStamped curr_cmd_4ws = *last_command_msg; const double steering_track = params_.track_ - 2 * params_.wheel_steering_y_offset_; const double joint_number = params_.wheel_names.size(); @@ -201,64 +207,121 @@ controller_interface::return_type FourSteeringController::update( realtime_odometry_transform_publisher_->unlockAndPublish(); } - curr_cmd_4ws.data.front_steering_angle = std::clamp(curr_cmd_4ws.data.front_steering_angle, -M_PI_4, M_PI_4); - curr_cmd_4ws.data.rear_steering_angle = std::clamp(curr_cmd_4ws.data.rear_steering_angle, -M_PI_4, M_PI_4); - - // Compute steering angles - const double tan_front_steering = tan(curr_cmd_4ws.data.front_steering_angle); - const double tan_rear_steering = tan(curr_cmd_4ws.data.rear_steering_angle); - const double steering_diff = steering_track*(tan_front_steering - tan_rear_steering)/2.0; - if(fabs(params_.wheel_base_ - fabs(steering_diff)) > 0.001) - { - front_left_steering = atan(params_.wheel_base_*tan_front_steering/(params_.wheel_base_-steering_diff)); - steer_cmd[0] = front_left_steering; - front_right_steering = atan(params_.wheel_base_*tan_front_steering/(params_.wheel_base_+steering_diff)); - steer_cmd[1] = front_right_steering; - rear_left_steering = atan(params_.wheel_base_*tan_rear_steering/(params_.wheel_base_-steering_diff)); - steer_cmd[2] = rear_left_steering; - rear_right_steering = atan(params_.wheel_base_*tan_rear_steering/(params_.wheel_base_+steering_diff)); - steer_cmd[3] = rear_right_steering; - } - - // Compute wheels velocities: - if(fabs(curr_cmd_4ws.data.speed) >= 0.0) - { - //Virutal front and rear wheelbase - // distance between the projection of the CIR on the wheelbase and the front axle - double l_front = 0; - if(fabs(tan(front_left_steering) - tan(front_right_steering)) > 0.01) + if(params_.enable_twist_cmd_ == true){ + //Limit velocities and accelerations: + limiter_lin_.limit(curr_cmd_twist.twist.linear.x, last0_cmd_.twist.linear.x, last1_cmd_.twist.linear.x, period.seconds()); + limiter_ang_.limit(curr_cmd_twist.twist.angular.z, last0_cmd_.twist.angular.z, last1_cmd_.twist.angular.z, period.seconds()); + last1_cmd_ = last0_cmd_; + last0_cmd_ = curr_cmd_twist; + + if(fabs(curr_cmd_twist.twist.linear.x) > 0.001) { - l_front = tan(front_right_steering) * tan(front_left_steering) * steering_track - / (tan(front_left_steering) - tan(front_right_steering)); + const double vel_steering_offset = (curr_cmd_twist.twist.angular.z * params_.wheel_steering_y_offset_) / params_.wheel_radius_; + const double sign = copysign(1.0, curr_cmd_twist.twist.linear.x); + vel_left_front = sign * std::hypot((curr_cmd_twist.twist.linear.x - curr_cmd_twist.twist.angular.z*steering_track/2), + (params_.wheel_base_*curr_cmd_twist.twist.angular.z/2.0)) / params_.wheel_radius_ + - vel_steering_offset; + wheel_vel_cmd[0] = vel_left_front; + vel_right_front = sign * std::hypot((curr_cmd_twist.twist.linear.x + curr_cmd_twist.twist.angular.z*steering_track/2), + (params_.wheel_base_*curr_cmd_twist.twist.angular.z/2.0)) / params_.wheel_radius_ + + vel_steering_offset; + wheel_vel_cmd[1] = vel_left_front; + vel_left_rear = sign * std::hypot((curr_cmd_twist.twist.linear.x - curr_cmd_twist.twist.angular.z*steering_track/2), + (params_.wheel_base_*curr_cmd_twist.twist.angular.z/2.0)) / params_.wheel_radius_ + - vel_steering_offset; + wheel_vel_cmd[2] = vel_left_front; + vel_right_rear = sign * std::hypot((curr_cmd_twist.twist.linear.x + curr_cmd_twist.twist.angular.z*steering_track/2), + (params_.wheel_base_*curr_cmd_twist.twist.angular.z/2.0)) / params_.wheel_radius_ + + vel_steering_offset; + wheel_vel_cmd[3] = vel_left_front; + } + // Compute steering angles: + if(fabs(2.0*curr_cmd_twist.twist.linear.x) > fabs(curr_cmd_twist.twist.angular.z*steering_track)) + { + front_left_steering = atan(curr_cmd_twist.twist.angular.z*params_.wheel_base_ / + (2.0*curr_cmd_twist.twist.linear.x - curr_cmd_twist.twist.angular.z*steering_track)); + steer_cmd[0] = front_left_steering; + front_right_steering = atan(curr_cmd_twist.twist.angular.z*params_.wheel_base_ / + (2.0*curr_cmd_twist.twist.linear.x + curr_cmd_twist.twist.angular.z*steering_track)); + steer_cmd[1] = front_left_steering; } - // distance between the projection of the CIR on the wheelbase and the rear axle - double l_rear = 0; - if(fabs(tan(rear_left_steering) - tan(rear_right_steering)) > 0.01) + else if(fabs(curr_cmd_twist.twist.linear.x) > 0.001) { - l_rear = tan(rear_right_steering) * tan(rear_left_steering) * steering_track - / (tan(rear_left_steering) - tan(rear_right_steering)); + front_left_steering = copysign(M_PI_2, curr_cmd_twist.twist.angular.z); + steer_cmd[0] = front_left_steering; + front_right_steering = copysign(M_PI_2, curr_cmd_twist.twist.angular.z); + steer_cmd[1] = front_left_steering; + } + rear_left_steering = -front_left_steering; + steer_cmd[2] = front_left_steering; + rear_right_steering = -front_right_steering; + steer_cmd[3] = front_left_steering; + } + else + { + // Limit velocities and accelerations: + limiter_lin_.limit(curr_cmd_4ws.data.speed, last0_cmd_4ws_.data.speed, last1_cmd_4ws_.data.speed, period.seconds()); + last1_cmd_4ws_ = last0_cmd_4ws_; + last0_cmd_4ws_ = curr_cmd_4ws; + curr_cmd_4ws.data.front_steering_angle = std::clamp(curr_cmd_4ws.data.front_steering_angle, -M_PI_4, M_PI_4); + curr_cmd_4ws.data.rear_steering_angle = std::clamp(curr_cmd_4ws.data.rear_steering_angle, -M_PI_4, M_PI_4); + + // Compute steering angles + const double tan_front_steering = tan(curr_cmd_4ws.data.front_steering_angle); + const double tan_rear_steering = tan(curr_cmd_4ws.data.rear_steering_angle); + const double steering_diff = steering_track*(tan_front_steering - tan_rear_steering)/2.0; + if(fabs(params_.wheel_base_ - fabs(steering_diff)) > 0.001) + { + front_left_steering = atan(params_.wheel_base_*tan_front_steering/(params_.wheel_base_-steering_diff)); + steer_cmd[0] = front_left_steering; + front_right_steering = atan(params_.wheel_base_*tan_front_steering/(params_.wheel_base_+steering_diff)); + steer_cmd[1] = front_right_steering; + rear_left_steering = atan(params_.wheel_base_*tan_rear_steering/(params_.wheel_base_-steering_diff)); + steer_cmd[2] = rear_left_steering; + rear_right_steering = atan(params_.wheel_base_*tan_rear_steering/(params_.wheel_base_+steering_diff)); + steer_cmd[3] = rear_right_steering; } - const double angular_speed_cmd = curr_cmd_4ws.data.speed * (tan_front_steering-tan_rear_steering)/params_.wheel_base_; - const double vel_steering_offset = (angular_speed_cmd*params_.wheel_steering_y_offset_)/params_.wheel_radius_; - const double sign = copysign(1.0, curr_cmd_4ws.data.speed); + // Compute wheels velocities: + if(fabs(curr_cmd_4ws.data.speed) > 0.001) + { + //Virutal front and rear wheelbase + // distance between the projection of the CIR on the wheelbase and the front axle + double l_front = 0; + if(fabs(tan(front_left_steering) - tan(front_right_steering)) > 0.01) + { + l_front = tan(front_right_steering) * tan(front_left_steering) * steering_track + / (tan(front_left_steering) - tan(front_right_steering)); + } + // distance between the projection of the CIR on the wheelbase and the rear axle + double l_rear = 0; + if(fabs(tan(rear_left_steering) - tan(rear_right_steering)) > 0.01) + { + l_rear = tan(rear_right_steering) * tan(rear_left_steering) * steering_track + / (tan(rear_left_steering) - tan(rear_right_steering)); + } - vel_left_front = sign * std::hypot((curr_cmd_4ws.data.speed - angular_speed_cmd*steering_track/2), - (l_front*angular_speed_cmd))/params_.wheel_radius_ - - vel_steering_offset; - wheel_vel_cmd[0] = vel_left_front; - vel_right_front = sign * std::hypot((curr_cmd_4ws.data.speed + angular_speed_cmd*steering_track/2), - (l_front*angular_speed_cmd))/params_.wheel_radius_ - + vel_steering_offset; - wheel_vel_cmd[1] = vel_right_front; - vel_left_rear = sign * std::hypot((curr_cmd_4ws.data.speed - angular_speed_cmd*steering_track/2), - (l_rear*angular_speed_cmd))/params_.wheel_radius_ - - vel_steering_offset; - wheel_vel_cmd[2] = vel_left_rear; - vel_right_rear = sign * std::hypot((curr_cmd_4ws.data.speed + angular_speed_cmd*steering_track/2), + const double angular_speed_cmd = curr_cmd_4ws.data.speed * (tan_front_steering-tan_rear_steering)/params_.wheel_base_; + const double vel_steering_offset = (angular_speed_cmd*params_.wheel_steering_y_offset_)/params_.wheel_radius_; + const double sign = copysign(1.0, curr_cmd_4ws.data.speed); + + vel_left_front = sign * std::hypot((curr_cmd_4ws.data.speed - angular_speed_cmd*steering_track/2), + (l_front*angular_speed_cmd))/params_.wheel_radius_ + - vel_steering_offset; + wheel_vel_cmd[0] = vel_left_front; + vel_right_front = sign * std::hypot((curr_cmd_4ws.data.speed + angular_speed_cmd*steering_track/2), + (l_front*angular_speed_cmd))/params_.wheel_radius_ + + vel_steering_offset; + wheel_vel_cmd[1] = vel_right_front; + vel_left_rear = sign * std::hypot((curr_cmd_4ws.data.speed - angular_speed_cmd*steering_track/2), (l_rear*angular_speed_cmd))/params_.wheel_radius_ - + vel_steering_offset; - wheel_vel_cmd[3] = vel_right_rear; + - vel_steering_offset; + wheel_vel_cmd[2] = vel_left_rear; + vel_right_rear = sign * std::hypot((curr_cmd_4ws.data.speed + angular_speed_cmd*steering_track/2), + (l_rear*angular_speed_cmd))/params_.wheel_radius_ + + vel_steering_offset; + wheel_vel_cmd[3] = vel_right_rear; + } } //Send steering cmds: @@ -302,18 +365,45 @@ controller_interface::CallbackReturn FourSteeringController::on_configure( odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size); const four_wheel_steering_msgs::msg::FourWheelSteeringStamped empty_cmd; + const geometry_msgs::msg::TwistStamped empty_twist_cmd; received_velocity_msg_ptr_.set(std::make_shared(empty_cmd)); + received_velocity_twist_msg_ptr_.set(std::make_shared(empty_twist_cmd)); // Fill last two commands with default constructed commands previous_commands_.emplace(empty_cmd); previous_commands_.emplace(empty_cmd); - - // initialize command subscriber - if(params_.use_stamped_cmd_) + previous_twist_commands_.emplace(empty_twist_cmd); + previous_twist_commands_.emplace(empty_twist_cmd); + // initialize command subscriber + if(params_.enable_twist_cmd_ == true) { - velocity_command_stamped_subscriber_ = - get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + if(params_.use_stamped_cmd_) + { + velocity_twist_cmd_stamped_subscriber_ = + get_node()->create_subscription( + DEFAULT_TWIST_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped command with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_twist_msg_ptr_.set(std::move(msg)); + } + ); + } else { + velocity_twist_cmd_unstamped_subscriber_ = get_node()->create_subscription( + DEFAULT_TWIST_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { @@ -321,35 +411,59 @@ controller_interface::CallbackReturn FourSteeringController::on_configure( get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); return; } - if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + + // Write fake header in the stored stamped command + std::shared_ptr twist_stamped; + received_velocity_twist_msg_ptr_.get(twist_stamped); + twist_stamped->twist = *msg; + twist_stamped->header.stamp = get_node()->get_clock()->now(); + }); + } + } else { + if(params_.use_stamped_cmd_) + { + velocity_command_stamped_subscriber_ = + get_node()->create_subscription( + DEFAULT_4WSCOMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { - RCLCPP_WARN_ONCE( - get_node()->get_logger(), - "Received FourWheelSteeringStamped with zero timestamp, setting it to current " - "time, this message will only be shown once"); - msg->header.stamp = get_node()->get_clock()->now(); + if (!subscriber_is_active_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received FourWheelSteeringStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.set(std::move(msg)); } - received_velocity_msg_ptr_.set(std::move(msg)); - } - ); - } else { - velocity_command_unstamped_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void - { - if (!subscriber_is_active_) + ); + } else { + velocity_command_unstamped_subscriber_ = get_node()->create_subscription( + DEFAULT_4WSCOMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - - // Write fake header in the stored stamped command - std::shared_ptr four_ws_stamped; - received_velocity_msg_ptr_.get(four_ws_stamped); - four_ws_stamped->data = *msg; - four_ws_stamped->header.stamp = get_node()->get_clock()->now(); - }); + if (!subscriber_is_active_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + + // Write fake header in the stored stamped command + std::shared_ptr four_ws_stamped; + received_velocity_msg_ptr_.get(four_ws_stamped); + four_ws_stamped->data = *msg; + four_ws_stamped->header.stamp = get_node()->get_clock()->now(); + }); + } + } // initialize odometry publisher and messasge @@ -474,8 +588,10 @@ bool FourSteeringController::reset() { // release the old queue std::queue empty_four_ws; - std::swap(previous_commands_, empty_four_ws); + std::queue empty_twist_; + std::swap(previous_commands_, empty_four_ws); + std::swap(previous_twist_commands_, empty_twist_); registered_wheel_handles_.clear(); registered_steer_handles_.clear(); diff --git a/four_steering_controller/src/four_steering_controller_parameter.yaml b/four_steering_controller/src/four_steering_controller_parameter.yaml index c553294e5e..7177b12162 100644 --- a/four_steering_controller/src/four_steering_controller_parameter.yaml +++ b/four_steering_controller/src/four_steering_controller_parameter.yaml @@ -93,4 +93,9 @@ four_steering_controller: type: bool, default_value: true, description: "If the command messages are stamped or not.", + } + enable_twist_cmd_: { + type: bool, + default_value: false, + description: "Choose which types of command to use ''twist'' or ''4ws_steering''.", } \ No newline at end of file From a601d8b4ad2b6e885ed391e1eeffae54c026a23c Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Sun, 25 Jun 2023 12:23:17 +0200 Subject: [PATCH 13/21] Update four steering controller for SteeringOdometryLibrary --- four_wheel_steering_controller/CMakeLists.txt | 0 .../four_wheel_steering_controller.hpp | 58 ++++++++++ .../visibility_control.h | 35 ++++++ four_wheel_steering_controller/package.xml | 0 .../src/four_wheel_steering_controller.cpp | 101 +++++++++++++++++ .../src/four_wheel_steering_controller.yaml | 0 .../steering_controllers_library.hpp | 1 + .../steering_odometry.hpp | 29 +++++ .../src/steering_controllers_library.cpp | 41 +++++++ .../src/steering_odometry.cpp | 102 ++++++++++++++++++ 10 files changed, 367 insertions(+) create mode 100644 four_wheel_steering_controller/CMakeLists.txt create mode 100644 four_wheel_steering_controller/include/four_wheel_steering_controller/four_wheel_steering_controller.hpp create mode 100644 four_wheel_steering_controller/include/four_wheel_steering_controller/visibility_control.h create mode 100644 four_wheel_steering_controller/package.xml create mode 100644 four_wheel_steering_controller/src/four_wheel_steering_controller.cpp create mode 100644 four_wheel_steering_controller/src/four_wheel_steering_controller.yaml diff --git a/four_wheel_steering_controller/CMakeLists.txt b/four_wheel_steering_controller/CMakeLists.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/four_wheel_steering_controller/include/four_wheel_steering_controller/four_wheel_steering_controller.hpp b/four_wheel_steering_controller/include/four_wheel_steering_controller/four_wheel_steering_controller.hpp new file mode 100644 index 0000000000..5afcd1b801 --- /dev/null +++ b/four_wheel_steering_controller/include/four_wheel_steering_controller/four_wheel_steering_controller.hpp @@ -0,0 +1,58 @@ +#ifndef FOUR_WHEEL_STEERING_CONTROLLER__FOUR_WHEEL_STEERING_CONTROLLER_HPP_ +#define FOUR_WHEEL_STEERING_CONTROLLER__FOUR_WHEEL_STEERING_CONTROLLER_HPP_ + +#include + +#include "four_wheel_steering_controller/visibility_control.h" +#include "four_wheel_steering_controller_parameters.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +namespace four_wheel_steering_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_FRONT_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_FRONT_LEFT_WHEEL = 1; +static constexpr size_t STATE_TRACTION_REAR_RIGHT_WHEEL = 2; +static constexpr size_t STATE_TRACTION_REAR_LEFT_WHEEL = 3; + +static constexpr size_t STATE_STEER_FRONT_RIGHT_WHEEL = 4; +static constexpr size_t STATE_STEER_FRONT_LEFT_WHEEL = 5; +static constexpr size_t STATE_STEER_REAR_RIGHT_WHEEL = 6; +static constexpr size_t STATE_STEER_REAR_LEFT_WHEEL = 7; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_FRONT_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_FRONT_LEFT_WHEEL = 1; +static constexpr size_t CMD_TRACTION_REAR_RIGHT_WHEEL = 2; +static constexpr size_t CMD_TRACTION_REAR_LEFT_WHEEL = 3; + +static constexpr size_t CMD_STEER_FRONT_RIGHT_WHEEL = 4; +static constexpr size_t CMD_STEER_FRONT_LEFT_WHEEL = 5; +static constexpr size_t CMD_STEER_REAR_RIGHT_WHEEL = 6; +static constexpr size_t CMD_STEER_REAR_LEFT_WHEEL = 7; + +static constexpr size_t NR_STATE_ITFS = 8; +static constexpr size_t NR_CMD_ITFS = 8; +static constexpr size_t NR_REF_ITFS = 2; + +class FourWheelSteeringController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + FourWheelSteeringController(); + + FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() override; + + FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC void + initialize_implementation_parameter_listener() override; + +protected: + std::shared_ptr four_wheel_steering_param_listener_; + four_wheel_steering_controller::Params four_wheel_steering_param_; +}; +} // namespace four_wheel_steering_controller + +#endif // FOUR_WHEEL_STEERING_CONTROLLER__FOUR_WHEEL_STEERING_CONTROLLER_HPP_ diff --git a/four_wheel_steering_controller/include/four_wheel_steering_controller/visibility_control.h b/four_wheel_steering_controller/include/four_wheel_steering_controller/visibility_control.h new file mode 100644 index 0000000000..a1e8cec64f --- /dev/null +++ b/four_wheel_steering_controller/include/four_wheel_steering_controller/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_EXPORT +#else +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC \ + FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_IMPORT +#endif +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_LOCAL +#else +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_LOCAL +#endif +#define FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // FOUR_WHEEL_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/four_wheel_steering_controller/package.xml b/four_wheel_steering_controller/package.xml new file mode 100644 index 0000000000..e69de29bb2 diff --git a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp new file mode 100644 index 0000000000..17e20cd99c --- /dev/null +++ b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp @@ -0,0 +1,101 @@ +// 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 "four_wheel_steering_controller/four_wheel_steering_controller.hpp" + +namespace four_wheel_steering_controller +{ +FourWheelSteeringController::FourWheelSteeringController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} + +void FourWheelSteeringController::initialize_implementation_parameter_listener() +{ + four_wheel_steering_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn FourWheelSteeringController::configure_odometry() +{ + four_wheel_steering_param_ = four_wheel_steering_param_listener_->get_params(); + + const double wheel_radius = four_wheel_steering_param_.wheel_radius; + const double wheel_track = four_wheel_steering_param_.wheel_track; + const double wheelbase = four_wheel_steering_param_.wheelbase; + const double y_steering_offset = four_wheel_steering_param_.y_steering_offset; + + odometry_.set_wheel_params(wheel_radius, wheelbase, wheel_track, y_steering_offset); + + odometry_.set_odometry_type(steering_odometry::FOUR_WHEEL_CONFIG); + + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + + RCLCPP_INFO(get_node()->get_logger(), "four_wheel odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool FourWheelSteeringController::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 front_right_wheel_value = state_interfaces_[STATE_TRACTION_FRONT_RIGHT_WHEEL].get_value(); + const double front_left_wheel_value = state_interfaces_[STATE_TRACTION_FRONT_LEFT_WHEEL].get_value(); + const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_REAR_RIGHT_WHEEL].get_value(); + const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_REAR_LEFT_WHEEL].get_value(); + + const double front_right_steer_position = state_interfaces_[STATE_STEER_FRONT_RIGHT_WHEEL].get_value(); + const double front_left_steer_position = state_interfaces_[STATE_STEER_FRONT_LEFT_WHEEL].get_value(); + const double rear_right_steer_position = state_interfaces_[STATE_STEER_REAR_RIGHT_WHEEL].get_value(); + const double rear_left_steer_position = state_interfaces_[STATE_STEER_REAR_LEFT_WHEEL].get_value(); + + if ( + !std::isnan(front_right_wheel_value) && !std::isnan(front_left_wheel_value) && + !std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) && + !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position) && + !std::isnan(rear_right_steer_position) && !std::isnan(rear_left_steer_position)) + { + if (params_.position_feedback) + { + double front_steer_position = 0.0; + if(fabs(front_right_steer_position) > 0.001 || fabs(front_left_steer_position) > 0.001) + { + front_steer_position = atan(2 * tan(front_right_steer_position) * tan(front_left_steer_position) / + (tan(front_right_steer_position) + tan(front_left_steer_position))); + } + double rear_steer_position = 0.0; + if(fabs(rear_right_steer_position) > 0.001 || fabs(rear_left_steer_position) > 0.001) + { + rear_steer_position = atan(2*tan(rear_right_steer_position)*tan(rear_left_steer_position)/ + (tan(rear_right_steer_position) + tan(rear_left_steer_position))); + } + // Estimate linear and angular velocity using joint information + odometry_.update_from_position( + front_right_wheel_value, front_left_wheel_value, rear_right_wheel_value, + rear_left_wheel_value, front_steer_position, rear_steer_position period.seconds()); + } + } + return true; +} +} // namespace four_wheel_steering_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + four_wheel_steering_controller::FourWheelSteeringController, + controller_interface::ChainableControllerInterface) diff --git a/four_wheel_steering_controller/src/four_wheel_steering_controller.yaml b/four_wheel_steering_controller/src/four_wheel_steering_controller.yaml new file mode 100644 index 0000000000..e69de29bb2 diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index b560e2a782..33c19b2eb1 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -36,6 +36,7 @@ // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "four_wheel_steering_msgs/msg/four_wheel_steering.hpp" #include "control_msgs/msg/steering_controller_status.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.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 index 3cfa056b27..35e02482ce 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -31,6 +31,8 @@ namespace steering_odometry const unsigned int BICYCLE_CONFIG = 0; const unsigned int TRICYCLE_CONFIG = 1; const unsigned int ACKERMANN_CONFIG = 2; +const unsigned int FOUR_STEERING_CONFIG = 3; + /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) @@ -123,6 +125,21 @@ class SteeringOdometry 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 wheels position + * \param fr_speed Front Right traction wheel velocity [rad/s] + * \param fl_speed Front Left traction wheel velocity [rad/s] + * \param rr_speed Rear Right steer wheel position [rad/s] + * \param rl_speed Rear Left steer wheel position [rad/s] + * \param front_steering Imaginary front steer wheel position [rad] + * \param rear_steering Imaginary rear steer wheel position [rad] + * \param dt time difference to last call + * \return true if the odometry is actually updated + */ + bool update_four_steering( + const double fr_speed, const double fl_speed, const double rr_speed, + const double rl_speed, const double front_steering, const double rear_steering, const double dt); + /** * \brief Updates the odometry class with latest velocity command * \param linear Linear velocity [m/s] @@ -172,6 +189,17 @@ class SteeringOdometry */ void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0); + /** + * \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 Sets the wheel parameters: radius, separation. wheelbase and wheel_steering_y_offset + */ + void set_wheel_params(double wheel_radius, double wheelbase = 0.0, + double wheel_track = 0.0, double y_steering_offset = 0.0); + /** * \brief Velocity rolling window size setter * \param velocity_rolling_window_size Velocity rolling window size @@ -245,6 +273,7 @@ class SteeringOdometry double wheel_track_; // [m] double wheelbase_; // [m] double wheel_radius_; // [m] + double y_steering_offset_; // [m] /// Configuration type used for the forward kinematics int config_type_ = -1; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index af2736a8a3..0b50989927 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -284,6 +284,19 @@ SteeringControllersLibrary::command_interface_configuration() const command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(nr_cmd_itfs_); + if (params_.four_steering) + { + for (size_t i = 0; i < params_.wheels_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + for (size_t i = 0; i < params_.steers_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.steers_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) @@ -325,6 +338,21 @@ SteeringControllersLibrary::state_interface_configuration() const const auto traction_wheels_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; + + if (params_.four_steering) + { + for (size_t i = 0; i < params_.wheels_names.size(); i++) + { + state_interfaces_config.names.push_back( + wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + } + for (size_t i = 0; i < params_.steers_names.size(); i++) + { + state_interfaces_config.names.push_back( + steers_names[i] + "/" + hardware_interface::HW_IF_POSITION); + } + } + if (params_.front_steering) { for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) @@ -447,6 +475,19 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c const double angular_command = reference_interfaces_[1]; auto [traction_commands, steering_commands] = odometry_.get_commands(linear_command, angular_command); + + if (params_.four_steering) + { + for (size_t i = 0; i < params_.wheels_names.size(); i++) + { + command_interfaces_[i].set_value(traction_commands[i]); + } + for (size_t i = 0; i < params_.steers_names.size(0; i++)) + { + command_interfaces_[i + params_wheels_names.size()].set_value(steering_commands[i]) + } + } + if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index c480dc1253..b63263b8d0 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -140,6 +140,57 @@ bool SteeringOdometry::update_from_position( return update_odometry(linear_velocity, angular, dt); } +bool SteeringOdometry::update_four_steering( + const double fr_speed, const double fl_speed, const double rr_speed, + const double rl_speed, const double front_steering, const double rear_steering, const double dt) +{ + const double front_tmp = cos(front_steering) * (tan(front_steering) - tan(rear_steering)) / wheelbase_; + + const double front_left_tmp = front_tmp / sqrt(1 - wheel_track_ * front_tmp * cos(front_steering) + +pow(wheel_track_*front_tmp / 2, 2)); + const double front_right_tmp = front_tmp / sqrt(1 + wheel_track_ * front_tmp * cos(front_steering) + +pow(wheel_track_*front_tmp / 2, 2)); + + const double fl_speed_tmp = fl_speed * (1 / (1 - y_steering_offset_ * front_left_tmp)); + const double fr_speed_tmp = fr_speed * (1 / (1 - y_steering_offset_ * front_right_tmp)); + + const double front_linear_speed = wheel_radius_ * copysign(1.0, fl_speed_tmp + fr_speed_tmp) * + sqrt((pow(fl_speed, 2) + pow(fr_speed, 2)) / (2 + pow(wheel_track_ * front_tmp, 2) / 2.0)); + + const double rear_tmp = cos(rear_steering) * (tan(front_steering) - tan(rear_steering)) / wheelbase_; + + const double rear_left_tmp = rear_tmp / sqrt(1 - wheel_track_ * rear_tmp * cos(rear_steering) + + pow(wheel_track_ * rear_tmp/ 2, 2)); + const double rear_right_tmp = rear_tmp / sqrt(1 + wheel_track_ * rear_tmp * cos(rear_steering) + +pow(wheel_track_ * rear_tmp/ 2, 2)); + + const double rl_speed_tmp = rl_speed * (1 / (1 - y_steering_offset_ * rear_left_tmp)); + const double rr_speed_tmp = rr_speed * (1 / (1 - y_steering_offset_ * rear_right_tmp)); + + const double rear_linear_speed = wheel_radius_ * copysign(1.0, rl_speed_tmp + rr_speed_tmp)* + sqrt((pow(rl_speed_tmp, 2) + pow(rr_speed_tmp, 2)) / (2 + pow(wheel_track_ * rear_tmp, 2) / 2.0)); + + angular_ = (front_linear_speed * front_tmp + rear_linear_speed * rear_tmp) / 2.0; + + linear_x_ = (front_linear_speed * cos(front_steering) + rear_linear_speed * cos(rear_steering)) / 2.0; + linear_y_ = (front_linear_speed * sin(front_steering) - wheelbase_ * angular_ / 2.0 + + rear_linear_speed * sin(rear_steering) + wheelbase_ * angular_ / 2.0) / 2.0; + + linear_velocity = copysign(1.0, rear_linear_speed)*sqrt(pow(linear_x_,2)+pow(linear_y_,2)); + + /// Integrate odometry: + // integrateXY(linear_x_*dt, linear_y_*dt, angular_*dt); + + // linear_accel_acc_((linear_vel_prev_ - linear_velocity)/dt); + // linear_vel_prev_ = linear_velocity; + // linear_jerk_acc_((linear_accel_prev_ - bacc::rolling_mean(linear_accel_acc_))/dt); + // linear_accel_prev_ = bacc::rolling_mean(linear_accel_acc_); + // front_steer_vel_acc_((front_steer_vel_prev_ - front_steering)/dt); + // front_steer_vel_prev_ = front_steering; + // rear_steer_vel_acc_((rear_steer_vel_prev_ - rear_steering)/dt); + // rear_steer_vel_prev_ = rear_steering; + return update_odometry(linear_velocity, angular_, dt); +} bool SteeringOdometry::update_from_velocity( const double traction_wheel_vel, const double steer_pos, const double dt) @@ -193,6 +244,14 @@ void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, d wheel_track_ = wheel_track; } +void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track, double y_steering_offset) +{ + wheel_radius_ = wheel_radius; + wheelbase_ = wheelbase; + wheel_track_ = wheel_track; + y_steering_offset_ = y_steering_offset; +} + void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) { velocity_rolling_window_size_ = velocity_rolling_window_size; @@ -278,6 +337,49 @@ std::tuple, std::vector> SteeringOdometry::get_comma } return std::make_tuple(traction_commands, steering_commands); } + else if (config_type_ == FOUR_STEERING_CONFIG) + { + std::vector traction_commands; + std::vector steering_commands; + + double steering_track = wheel_track_- 2 * y_steering_offset_; + double vel_steering_offset = (alpha * y_steering_offset_) / wheel_radius_; + double sign = copysign(1.0, Ws); + double vel_left_front = sign * std::hypot((Ws - alpha * steering_track / 2), + (wheelbase_ * alpha / 2.0)) / wheel_radius_ + - vel_steering_offset; + double vel_right_front = sign * std::hypot((Ws + alpha * steering_track / 2), + (wheelbase_ * alpha / 2.0)) / wheel_radius_ + + vel_steering_offset; + double vel_left_rear = sign * std::hypot((Ws - alpha * steering_track / 2), + (wheelbase_ * alpha / 2.0)) / wheel_radius_ + - vel_steering_offset; + double vel_right_rear = sign * std::hypot((Ws + alpha * steering_track / 2), + (wheelbase_ * alpha / 2.0)) / wheel_radius_ + + vel_steering_offset; + traction_commands = {vel_left_front, vel_right_front, vel_left_rear, vel_right_rear}; + + double front_left_steering = 0.0; + double front_right_steering = 0.0; + // Compute steering angles + if(fabs(2.0 * Ws) > fabs(alpha * steering_track)) + { + front_left_steering = atan(alpha * wheelbase_ / + (2.0 * Ws - alpha * steering_track)); + front_right_steering = atan(alpha * wheelbase_ / + (2.0 * Ws + alpha * steering_track)); + } + else if(fabs(Ws) > 0.001) + { + front_left_steering = copysign(M_PI_2, alpha); + front_right_steering = copysign(M_PI_2, alpha); + } + double rear_left_steering = -front_left_steering; + double rear_right_steering = -front_right_steering; + steering_commands = {front_left_steering, front_right_steering, rear_left_steering, rear_right_steering}; + + return std::make_tuple(traction_commands, steering_commands); + } else { throw std::runtime_error("Config not implemented"); From cbdbe4e1df0a7f11d05ace73ac87213d7811e253 Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Sun, 25 Jun 2023 12:38:55 +0200 Subject: [PATCH 14/21] small updates --- four_wheel_steering_controller/CMakeLists.txt | 107 ++++++++++++++++++ .../four_wheel_steering_controller.xml | 8 ++ four_wheel_steering_controller/package.xml | 31 +++++ 3 files changed, 146 insertions(+) create mode 100644 four_wheel_steering_controller/four_wheel_steering_controller.xml diff --git a/four_wheel_steering_controller/CMakeLists.txt b/four_wheel_steering_controller/CMakeLists.txt index e69de29bb2..97e9e85978 100644 --- a/four_wheel_steering_controller/CMakeLists.txt +++ b/four_wheel_steering_controller/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.16) +project(four_wheel_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(four_wheel_steering_controller_parameters + src/four_wheel_steering_controller.yaml +) + +add_library( + four_wheel_steering_controller + SHARED + src/four_wheel_steering_controller.cpp +) +target_compile_features(four_wheel_steering_controller PUBLIC cxx_std_17) +target_include_directories(four_wheel_steering_controller PUBLIC + $ + $) +target_link_libraries(four_wheel_steering_controller PUBLIC + four_wheel_steering_controller_parameters) +ament_target_dependencies(four_wheel_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(four_wheel_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface four_wheel_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_four_wheel_steering_controller + test/test_load_four_wheel_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_params.yaml + ) + ament_target_dependencies(test_load_four_wheel_steering_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_four_wheel_steering_controller + test/test_four_wheel_steering_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_params.yaml + ) + target_include_directories(test_four_wheel_steering_controller PRIVATE include) + target_link_libraries(test_four_wheel_steering_controller four_wheel_steering_controller) + ament_target_dependencies( + test_four_wheel_steering_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_four_wheel_steering_controller_preceeding test/test_four_wheel_steering_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_preceeding_params.yaml) + target_include_directories(test_four_wheel_steering_controller_preceeding PRIVATE include) + target_link_libraries(test_four_wheel_steering_controller_preceeding four_wheel_steering_controller) + ament_target_dependencies( + test_four_wheel_steering_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/four_wheel_steering_controller +) + +install( + TARGETS four_wheel_steering_controller four_wheel_steering_controller_parameters + EXPORT export_four_wheel_steering_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_four_wheel_steering_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/four_wheel_steering_controller/four_wheel_steering_controller.xml b/four_wheel_steering_controller/four_wheel_steering_controller.xml new file mode 100644 index 0000000000..4ab60b7939 --- /dev/null +++ b/four_wheel_steering_controller/four_wheel_steering_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for 4 WheelSteering robotic platform with four traction and four steering joints. + + + diff --git a/four_wheel_steering_controller/package.xml b/four_wheel_steering_controller/package.xml index e69de29bb2..ed75ca0eac 100644 --- a/four_wheel_steering_controller/package.xml +++ b/four_wheel_steering_controller/package.xml @@ -0,0 +1,31 @@ + + + + ackermann_steering_controller + 3.10.1 + Steering controller for Four-Wheels kinematics. All wheels can steer and drive the robotic platform. + Apache License 2.0 + Davide Graziato + + 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 + + From 78772a5607173b52bf22fcb6bc847d5b87e6d0d1 Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Sun, 25 Jun 2023 13:55:56 +0200 Subject: [PATCH 15/21] bug fixes and improvements --- four_wheel_steering_controller/CMakeLists.txt | 74 +++++++++---------- four_wheel_steering_controller/package.xml | 2 +- .../src/four_wheel_steering_controller.cpp | 10 ++- .../src/four_wheel_steering_controller.yaml | 29 ++++++++ .../steering_controllers_library.hpp | 2 +- .../steering_odometry.hpp | 35 ++++----- 6 files changed, 89 insertions(+), 63 deletions(-) diff --git a/four_wheel_steering_controller/CMakeLists.txt b/four_wheel_steering_controller/CMakeLists.txt index 97e9e85978..775dc5ecda 100644 --- a/four_wheel_steering_controller/CMakeLists.txt +++ b/four_wheel_steering_controller/CMakeLists.txt @@ -43,51 +43,51 @@ ament_target_dependencies(four_wheel_steering_controller PUBLIC ${THIS_PACKAGE_I # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(four_wheel_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL") +target_compile_definitions(four_wheel_steering_controller PRIVATE "FOUR_WHEEL_STEERING_CONTROLLER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface four_wheel_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) +# 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_four_wheel_steering_controller - test/test_load_four_wheel_steering_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_params.yaml - ) - ament_target_dependencies(test_load_four_wheel_steering_controller - controller_manager - hardware_interface - ros2_control_test_assets - ) +# add_rostest_with_parameters_gmock(test_load_four_wheel_steering_controller +# test/test_load_four_wheel_steering_controller.cpp +# ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_params.yaml +# ) +# ament_target_dependencies(test_load_four_wheel_steering_controller +# controller_manager +# hardware_interface +# ros2_control_test_assets +# ) - add_rostest_with_parameters_gmock(test_four_wheel_steering_controller - test/test_four_wheel_steering_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_params.yaml - ) - target_include_directories(test_four_wheel_steering_controller PRIVATE include) - target_link_libraries(test_four_wheel_steering_controller four_wheel_steering_controller) - ament_target_dependencies( - test_four_wheel_steering_controller - controller_interface - hardware_interface - ) +# add_rostest_with_parameters_gmock(test_four_wheel_steering_controller +# test/test_four_wheel_steering_controller.cpp +# ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_params.yaml +# ) +# target_include_directories(test_four_wheel_steering_controller PRIVATE include) +# target_link_libraries(test_four_wheel_steering_controller four_wheel_steering_controller) +# ament_target_dependencies( +# test_four_wheel_steering_controller +# controller_interface +# hardware_interface +# ) - add_rostest_with_parameters_gmock( - test_four_wheel_steering_controller_preceeding test/test_four_wheel_steering_controller_preceeding.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_preceeding_params.yaml) - target_include_directories(test_four_wheel_steering_controller_preceeding PRIVATE include) - target_link_libraries(test_four_wheel_steering_controller_preceeding four_wheel_steering_controller) - ament_target_dependencies( - test_four_wheel_steering_controller_preceeding - controller_interface - hardware_interface - ) -endif() +# add_rostest_with_parameters_gmock( +# test_four_wheel_steering_controller_preceeding test/test_four_wheel_steering_controller_preceeding.cpp +# ${CMAKE_CURRENT_SOURCE_DIR}/test/four_wheel_steering_controller_preceeding_params.yaml) +# target_include_directories(test_four_wheel_steering_controller_preceeding PRIVATE include) +# target_link_libraries(test_four_wheel_steering_controller_preceeding four_wheel_steering_controller) +# ament_target_dependencies( +# test_four_wheel_steering_controller_preceeding +# controller_interface +# hardware_interface +# ) +# endif() install( DIRECTORY include/ diff --git a/four_wheel_steering_controller/package.xml b/four_wheel_steering_controller/package.xml index ed75ca0eac..91d9c7d8d0 100644 --- a/four_wheel_steering_controller/package.xml +++ b/four_wheel_steering_controller/package.xml @@ -1,7 +1,7 @@ - ackermann_steering_controller + four_wheel_steering_controller 3.10.1 Steering controller for Four-Wheels kinematics. All wheels can steer and drive the robotic platform. Apache License 2.0 diff --git a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp index 17e20cd99c..244e403a26 100644 --- a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp +++ b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp @@ -38,7 +38,7 @@ controller_interface::CallbackReturn FourWheelSteeringController::configure_odom odometry_.set_wheel_params(wheel_radius, wheelbase, wheel_track, y_steering_offset); - odometry_.set_odometry_type(steering_odometry::FOUR_WHEEL_CONFIG); + odometry_.set_odometry_type(steering_odometry::FOUR_STEERING_CONFIG); set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); @@ -85,13 +85,15 @@ bool FourWheelSteeringController::update_odometry(const rclcpp::Duration & perio (tan(rear_right_steer_position) + tan(rear_left_steer_position))); } // Estimate linear and angular velocity using joint information - odometry_.update_from_position( + odometry_.update_four_steering( front_right_wheel_value, front_left_wheel_value, rear_right_wheel_value, - rear_left_wheel_value, front_steer_position, rear_steer_position period.seconds()); + rear_left_wheel_value, front_steer_position, rear_steer_position, period.seconds()); + } } - } return true; + } } + } // namespace four_wheel_steering_controller #include "pluginlib/class_list_macros.hpp" diff --git a/four_wheel_steering_controller/src/four_wheel_steering_controller.yaml b/four_wheel_steering_controller/src/four_wheel_steering_controller.yaml index e69de29bb2..22182869d9 100644 --- a/four_wheel_steering_controller/src/four_wheel_steering_controller.yaml +++ b/four_wheel_steering_controller/src/four_wheel_steering_controller.yaml @@ -0,0 +1,29 @@ +four_wheel_steering_controller: + wheel_radius: + { + type: double, + default_value: 0.0, + description: "Wheel radius.", + read_only: false, + } + wheel_track: + { + type: double, + default_value: 0.0, + description: "Wheels track length.", + read_only: false, + } + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheels.", + read_only: false, + } + y_steering_offset: + { + type: double, + default_value: 0.0, + description: "Offest between the steering and wheel joints.", + read_only: false, + } diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 33c19b2eb1..ad6f9bb32a 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -36,7 +36,7 @@ // TODO(anyone): Replace with controller specific messages #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" -#include "four_wheel_steering_msgs/msg/four_wheel_steering.hpp" +//#include "four_wheel_steering_msgs/msg/four_wheel_steering.hpp" #include "control_msgs/msg/steering_controller_status.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.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 index 35e02482ce..2ce61d07a7 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -90,6 +90,21 @@ class SteeringOdometry 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 fr_speed Traction wheel velocity [rad/s] + * \param fl_speed Steer wheel position [rad] + * \param rr_speed time difference to last call + * \param rl_speed Traction wheel velocity [rad/s] + * \param front_steering Steer wheel position [rad] + * \param rear_steering time difference to last call + * \param dt Steer wheel position [rad] + * \return true if the odometry is actually updated + */ + bool update_four_steering( + const double fr_speed, const double fl_speed, const double rr_speed, + const double rl_speed, const double front_steering, const double rear_steering, const double dt); + /** * \brief Updates the odometry class with latest wheels position * \param traction_wheel_vel Traction wheel velocity [rad/s] @@ -125,21 +140,6 @@ class SteeringOdometry 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 wheels position - * \param fr_speed Front Right traction wheel velocity [rad/s] - * \param fl_speed Front Left traction wheel velocity [rad/s] - * \param rr_speed Rear Right steer wheel position [rad/s] - * \param rl_speed Rear Left steer wheel position [rad/s] - * \param front_steering Imaginary front steer wheel position [rad] - * \param rear_steering Imaginary rear steer wheel position [rad] - * \param dt time difference to last call - * \return true if the odometry is actually updated - */ - bool update_four_steering( - const double fr_speed, const double fl_speed, const double rr_speed, - const double rl_speed, const double front_steering, const double rear_steering, const double dt); - /** * \brief Updates the odometry class with latest velocity command * \param linear Linear velocity [m/s] @@ -189,11 +189,6 @@ class SteeringOdometry */ void set_wheel_params(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0); - /** - * \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 Sets the wheel parameters: radius, separation. wheelbase and wheel_steering_y_offset */ From 6d556ad9e64c919aebfcafe21386d039837d5171 Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Sun, 25 Jun 2023 16:31:30 +0200 Subject: [PATCH 16/21] bug fixes --- .../src/four_wheel_steering_controller.cpp | 3 +- .../steering_controllers_library.hpp | 2 ++ .../src/steering_controllers_library.cpp | 8 ++--- .../src/steering_controllers_library.yaml | 29 +++++++++++++++++++ .../src/steering_odometry.cpp | 6 ++-- 5 files changed, 39 insertions(+), 9 deletions(-) diff --git a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp index 244e403a26..40df7eadf1 100644 --- a/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp +++ b/four_wheel_steering_controller/src/four_wheel_steering_controller.cpp @@ -90,10 +90,9 @@ bool FourWheelSteeringController::update_odometry(const rclcpp::Duration & perio rear_left_wheel_value, front_steer_position, rear_steer_position, period.seconds()); } } - return true; + return true; } } - } // namespace four_wheel_steering_controller #include "pluginlib/class_list_macros.hpp" diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index ad6f9bb32a..b244b59236 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -139,6 +139,8 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl std::vector rear_wheels_state_names_; std::vector front_wheels_state_names_; + std::vector wheels_names_; + std::vector steers_names_; private: // callback for topic interface diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 0b50989927..ca64619f5b 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -344,12 +344,12 @@ SteeringControllersLibrary::state_interface_configuration() const for (size_t i = 0; i < params_.wheels_names.size(); i++) { state_interfaces_config.names.push_back( - wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); + wheels_names_[i] + "/" + hardware_interface::HW_IF_VELOCITY); } for (size_t i = 0; i < params_.steers_names.size(); i++) { state_interfaces_config.names.push_back( - steers_names[i] + "/" + hardware_interface::HW_IF_POSITION); + steers_names_[i] + "/" + hardware_interface::HW_IF_POSITION); } } @@ -482,9 +482,9 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { command_interfaces_[i].set_value(traction_commands[i]); } - for (size_t i = 0; i < params_.steers_names.size(0; i++)) + for (size_t i = 0; i < params_.steers_names.size(); i++) { - command_interfaces_[i + params_wheels_names.size()].set_value(steering_commands[i]) + command_interfaces_[i + params_.wheels_names.size()].set_value(steering_commands[i]); } } diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 86acb01dae..d5ab4d4338 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -12,6 +12,13 @@ steering_controllers_library: read_only: true, } + four_steering: { + type: bool, + default_value: true, + description: "Is a four wheel steering robot?", + read_only: true, + } + rear_wheels_names: { type: string_array, description: "Names of rear wheel joints.", @@ -34,6 +41,28 @@ steering_controllers_library: } } + wheels_names: { + type: string_array, + description: "Names of wheel joints.", + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + not_empty<>: null, + } + } + + steers_names: { + type: string_array, + description: "Names of steer 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.", diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index b63263b8d0..31a4030f5f 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -172,11 +172,11 @@ bool SteeringOdometry::update_four_steering( angular_ = (front_linear_speed * front_tmp + rear_linear_speed * rear_tmp) / 2.0; - linear_x_ = (front_linear_speed * cos(front_steering) + rear_linear_speed * cos(rear_steering)) / 2.0; - linear_y_ = (front_linear_speed * sin(front_steering) - wheelbase_ * angular_ / 2.0 + const double linear_x_ = (front_linear_speed * cos(front_steering) + rear_linear_speed * cos(rear_steering)) / 2.0; + const double linear_y_ = (front_linear_speed * sin(front_steering) - wheelbase_ * angular_ / 2.0 + rear_linear_speed * sin(rear_steering) + wheelbase_ * angular_ / 2.0) / 2.0; - linear_velocity = copysign(1.0, rear_linear_speed)*sqrt(pow(linear_x_,2)+pow(linear_y_,2)); + const double linear_velocity = copysign(1.0, rear_linear_speed)*sqrt(pow(linear_x_,2)+pow(linear_y_,2)); /// Integrate odometry: // integrateXY(linear_x_*dt, linear_y_*dt, angular_*dt); From c387ba165b5107f91acd5974728b44c64779e6b2 Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Tue, 4 Jul 2023 23:25:40 +0200 Subject: [PATCH 17/21] small bugfixes --- .../steering_controllers_library/steering_odometry.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 2ce61d07a7..2d83fc4264 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -192,8 +192,8 @@ class SteeringOdometry /** * \brief Sets the wheel parameters: radius, separation. wheelbase and wheel_steering_y_offset */ - void set_wheel_params(double wheel_radius, double wheelbase = 0.0, - double wheel_track = 0.0, double y_steering_offset = 0.0); + void set_wheel_params(double wheel_radius, double wheelbase, + double wheel_track, double y_steering_offset); /** * \brief Velocity rolling window size setter From 63c08f004844b0953239f3a4f056d40340e5a6bd Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Tue, 4 Jul 2023 23:25:40 +0200 Subject: [PATCH 18/21] small bugfixes --- .../steering_odometry.hpp | 287 ++++++++++++++++++ 1 file changed, 287 insertions(+) create mode 100644 steering_controllers_library/include/steering_controllers_library/steering_odometry.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..2d83fc4264 --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -0,0 +1,287 @@ +// 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; +const unsigned int FOUR_STEERING_CONFIG = 3; + +/** + * \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 fr_speed Traction wheel velocity [rad/s] + * \param fl_speed Steer wheel position [rad] + * \param rr_speed time difference to last call + * \param rl_speed Traction wheel velocity [rad/s] + * \param front_steering Steer wheel position [rad] + * \param rear_steering time difference to last call + * \param dt Steer wheel position [rad] + * \return true if the odometry is actually updated + */ + bool update_four_steering( + const double fr_speed, const double fl_speed, const double rr_speed, + const double rl_speed, const double front_steering, const double rear_steering, 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 Sets the wheel parameters: radius, separation. wheelbase and wheel_steering_y_offset + */ + void set_wheel_params(double wheel_radius, double wheelbase, + double wheel_track, double y_steering_offset); + + /** + * \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] + double y_steering_offset_; // [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_ From 3431d9a30745cdd7acc8152db9b5d5c4badbf65a Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Mon, 17 Jul 2023 23:09:00 +0200 Subject: [PATCH 19/21] Revert "small bugfixes" This reverts commit 63c08f004844b0953239f3a4f056d40340e5a6bd. --- .../steering_odometry.hpp | 287 ------------------ 1 file changed, 287 deletions(-) delete mode 100644 steering_controllers_library/include/steering_controllers_library/steering_odometry.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 deleted file mode 100644 index 2d83fc4264..0000000000 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ /dev/null @@ -1,287 +0,0 @@ -// 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; -const unsigned int FOUR_STEERING_CONFIG = 3; - -/** - * \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 fr_speed Traction wheel velocity [rad/s] - * \param fl_speed Steer wheel position [rad] - * \param rr_speed time difference to last call - * \param rl_speed Traction wheel velocity [rad/s] - * \param front_steering Steer wheel position [rad] - * \param rear_steering time difference to last call - * \param dt Steer wheel position [rad] - * \return true if the odometry is actually updated - */ - bool update_four_steering( - const double fr_speed, const double fl_speed, const double rr_speed, - const double rl_speed, const double front_steering, const double rear_steering, 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 Sets the wheel parameters: radius, separation. wheelbase and wheel_steering_y_offset - */ - void set_wheel_params(double wheel_radius, double wheelbase, - double wheel_track, double y_steering_offset); - - /** - * \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] - double y_steering_offset_; // [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_ From 7eb5c909c4f7f4bbb465558505e2e2adb4ddc42a Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Tue, 4 Jul 2023 23:25:40 +0200 Subject: [PATCH 20/21] small bugfixes --- .../steering_odometry.hpp | 287 ++++++++++++++++++ 1 file changed, 287 insertions(+) create mode 100644 steering_controllers_library/include/steering_controllers_library/steering_odometry.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..2d83fc4264 --- /dev/null +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -0,0 +1,287 @@ +// 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; +const unsigned int FOUR_STEERING_CONFIG = 3; + +/** + * \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 fr_speed Traction wheel velocity [rad/s] + * \param fl_speed Steer wheel position [rad] + * \param rr_speed time difference to last call + * \param rl_speed Traction wheel velocity [rad/s] + * \param front_steering Steer wheel position [rad] + * \param rear_steering time difference to last call + * \param dt Steer wheel position [rad] + * \return true if the odometry is actually updated + */ + bool update_four_steering( + const double fr_speed, const double fl_speed, const double rr_speed, + const double rl_speed, const double front_steering, const double rear_steering, 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 Sets the wheel parameters: radius, separation. wheelbase and wheel_steering_y_offset + */ + void set_wheel_params(double wheel_radius, double wheelbase, + double wheel_track, double y_steering_offset); + + /** + * \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] + double y_steering_offset_; // [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_ From fe37b18b17cc9b29c29b3e6d44df0985619484a1 Mon Sep 17 00:00:00 2001 From: Davide Graziato Date: Mon, 17 Jul 2023 23:37:18 +0200 Subject: [PATCH 21/21] bug fix --- .../src/steering_controllers_library.cpp | 37 +++++++++++-------- 1 file changed, 21 insertions(+), 16 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index ca64619f5b..412b36e9b0 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -296,7 +296,9 @@ SteeringControllersLibrary::command_interface_configuration() const command_interfaces_config.names.push_back( params_.steers_names[i] + "/" + hardware_interface::HW_IF_POSITION); } + return command_interfaces_config; } + if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) @@ -351,6 +353,7 @@ SteeringControllersLibrary::state_interface_configuration() const state_interfaces_config.names.push_back( steers_names_[i] + "/" + hardware_interface::HW_IF_POSITION); } + return state_interfaces_config; } if (params_.front_steering) @@ -487,29 +490,31 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c command_interfaces_[i + params_.wheels_names.size()].set_value(steering_commands[i]); } } - - 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 { + if (params_.front_steering) { - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + 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_.rear_wheels_names.size(); 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 + { { - command_interfaces_[i + params_.front_wheels_names.size()].set_value( - steering_commands[i]); + 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]); + } } } }