From 1b6c0f111704bbc279bc04c468ad01d8496afdc6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 3 Nov 2024 20:03:32 +0100 Subject: [PATCH 1/7] Example3: Add section on command mode switching (#624) * Add section on command mode switching * Update example_3/doc/userdoc.rst Co-authored-by: Sai Kishor Kothakota --------- Co-authored-by: Sai Kishor Kothakota --- example_3/doc/userdoc.rst | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/example_3/doc/userdoc.rst b/example_3/doc/userdoc.rst index 29570224..ac362566 100644 --- a/example_3/doc/userdoc.rst +++ b/example_3/doc/userdoc.rst @@ -142,7 +142,30 @@ Tutorial steps [ros2_control_node-1] pos: 0.67, vel: 5.00, acc: 0.00 for joint 0 [ros2_control_node-1] pos: 0.67, vel: 5.00, acc: 0.00 for joint 1 -6. To demonstrate illegal controller configuration, use one of the following launch file arguments: +6. Now you can also switch controllers during runtime, which also changes the command mode automatically. First, you have to load the new controller, for example the ``forward_position_controller`` if you haven't changed the launch file argument. + + .. code-block:: shell + + ros2 control load_controller forward_position_controller $(ros2 pkg prefix ros2_control_demo_example_3 --share)/config/rrbot_multi_interface_forward_controllers.yaml + ros2 control set_controller_state forward_position_controller inactive + + Then you can switch controllers using the following command: + + .. code-block:: shell + + ros2 control switch_controllers --deactivate forward_velocity_controller --activate forward_position_controller + + Observe the output of the following CLI commands, and see how the command interfaces are claimed by the new controller. + + .. code-block:: shell + + ros2 control list_controllers + ros2 control list_hardware_interfaces + + Try now to send commands to the new controller, as described in the previous step. + + +7. To demonstrate illegal controller configuration, use one of the following launch file arguments: * ``robot_controller:=forward_illegal1_controller`` or * ``robot_controller:=forward_illegal2_controller`` From 7c654908d57a9d6042ee095c5fdce7947b88902a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 17 Nov 2024 18:08:58 +0100 Subject: [PATCH 2/7] Don't test main on semi-binary (#642) --- .github/workflows/humble-semi-binary-build.yml | 2 +- .github/workflows/iron-semi-binary-build.yml | 2 +- .github/workflows/rolling-semi-binary-build.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 5ed87435..01aa6d49 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [humble] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index 9ca613d1..400bc327 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -40,7 +40,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [iron] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index ff3d0788..c8d4f8dc 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: fail-fast: false matrix: ROS_DISTRO: [rolling, jazzy] - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: ${{ matrix.ROS_DISTRO }} ros_repo: ${{ matrix.ROS_REPO }} From 8bddea651c10401b9e57eeed65f003b9ae6cb3e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 17 Nov 2024 21:29:56 +0100 Subject: [PATCH 3/7] Fix several links (#645) --- example_13/doc/userdoc.rst | 5 +++- example_15/doc/userdoc.rst | 57 +++++++++++++++++++++----------------- 2 files changed, 36 insertions(+), 26 deletions(-) diff --git a/example_13/doc/userdoc.rst b/example_13/doc/userdoc.rst index b09fce9b..e9e28c79 100644 --- a/example_13/doc/userdoc.rst +++ b/example_13/doc/userdoc.rst @@ -462,7 +462,10 @@ Files used for this demos - URDF file: `three_robots.urdf.xacro `__ + Description: `threedofbot_description.urdf.xacro `__ - + ``ros2_control`` tag: `three_robots.ros2_control.xacro `__ + + ``ros2_control`` tag: + + `threedofbot.ros2_control.xacro `__ + + `rrbot_system_position_only.ros2_control.xacro `__ + + `rrbot_system_with_sensor.ros2_control.xacro `__ - RViz configuration: `three_robots.rviz `__ Controllers from this demo diff --git a/example_15/doc/userdoc.rst b/example_15/doc/userdoc.rst index 55a9ae20..dc73e704 100644 --- a/example_15/doc/userdoc.rst +++ b/example_15/doc/userdoc.rst @@ -12,21 +12,6 @@ This example shows how to integrate multiple robots under different controller m Scenario: Using ros2_control within a local namespace ----------------------------------------------------- -* Launch file: `rrbot_namespace.launch.py `__ -* Controllers yaml: `rrbot_namespace_controllers.yaml `__ -* URDF file: `rrbot.urdf.xacro `__ - - * Description: `rrbot_description.urdf.xacro `__ - * ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ - -* RViz configuration: `rrbot.rviz `__ -* Test nodes goals configuration: - - + `rrbot_forward_position_publisher `__ - + `rrbot_joint_trajectory_publisher `__ - -* Hardware interface plugin: `rrbot.cpp `__ - .. note:: When running ``ros2 control`` CLI commands you have to use additional parameter with exact controller manager node name, i.e., ``-c /rrbot/controller_manager``. @@ -75,24 +60,26 @@ Commanding the robot using ``JointTrajectoryController`` (name: ``/rrbot/positio ros2 launch ros2_control_demo_example_15 test_joint_trajectory_controller.launch.py publisher_config:=rrbot_namespace_joint_trajectory_publisher.yaml -Scenario: Using multiple controller managers on the same machine ----------------------------------------------------------------- +Files used for this demo: -* Launch file: `multi_controller_manager_example_two_rrbots.launch.py `__ -* Controllers yaml: - - `multi_controller_manager_generic_controllers.yaml `__ -* URDF file: `rrbot.urdf.xacro `__ +* Launch file: `rrbot_namespace.launch.py `__ +* Controllers yaml: `rrbot_namespace_controllers.yaml `__ +* URDF file: `rrbot.urdf.xacro `__ * Description: `rrbot_description.urdf.xacro `__ - * ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ + * ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ * RViz configuration: `rrbot.rviz `__ * Test nodes goals configuration: - + `rrbot_forward_position_publisher `__ - + `rrbot_joint_trajectory_publisher `__ + + `rrbot_forward_position_publisher `__ + + `rrbot_joint_trajectory_publisher `__ -* Hardware interface plugin: `rrbot.cpp `__ +* Hardware interface plugin: `rrbot.cpp `__ + + +Scenario: Using multiple controller managers on the same machine +---------------------------------------------------------------- .. note:: @@ -168,6 +155,26 @@ Commanding the robots using the now activated ``position_trajectory_controller`` ros2 launch ros2_control_demo_example_15 test_multi_controller_manager_joint_trajectory_controller.launch.py + +Files used for this demo: + +* Launch file: `multi_controller_manager_example_two_rrbots.launch.py `__ +* Controllers yaml: + - `multi_controller_manager_rrbot_generic_controllers.yaml `__ +* URDF file: `rrbot.urdf.xacro `__ + + * Description: `rrbot_description.urdf.xacro `__ + * ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ + +* RViz configuration: `rrbot.rviz `__ +* Test nodes goals configuration: + + + `rrbot_forward_position_publisher `__ + + `rrbot_joint_trajectory_publisher `__ + +* Hardware interface plugin: `rrbot.cpp `__ + + Controllers from this demo -------------------------- * ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ From 0c700923f4da4e239905a05347840e8df828fea4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 17 Nov 2024 23:54:07 +0100 Subject: [PATCH 4/7] Add paths filter to humble docker build job (#648) * Add paths filter * Rename file --- ...ker-build.yaml => humble-docker-build.yml} | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) rename .github/workflows/{humble-docker-build.yaml => humble-docker-build.yml} (50%) diff --git a/.github/workflows/humble-docker-build.yaml b/.github/workflows/humble-docker-build.yml similarity index 50% rename from .github/workflows/humble-docker-build.yaml rename to .github/workflows/humble-docker-build.yml index ad9b2b57..b9eb118a 100644 --- a/.github/workflows/humble-docker-build.yaml +++ b/.github/workflows/humble-docker-build.yml @@ -5,9 +5,31 @@ on: pull_request: branches: - humble + paths: + - '**.hpp' + - '**.cpp' + - Dockerfile/** + - '.github/workflows/humble-docker-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - '**.xacro' + - '**.py' + - '**.yaml' + - 'ros2_control_demos.humble.repos' push: branches: - humble + paths: + - '**.hpp' + - '**.cpp' + - Dockerfile/** + - '.github/workflows/humble-docker-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - '**.xacro' + - '**.py' + - '**.yaml' + - 'ros2_control_demos.humble.repos' schedule: # Run every morning to detect broken dependencies - cron: '40 1 * * *' From 7b7f70c992e1ab6a42ec095a24f2c731573d4e28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 22:10:34 +0100 Subject: [PATCH 5/7] Remove iron workflows and update readme (#612) --- .github/dependabot.yml | 7 --- .github/mergify.yml | 9 ---- .github/workflows/iron-binary-build.yml | 48 -------------------- .github/workflows/iron-check-docs.yml | 17 ------- .github/workflows/iron-docker-build.yaml | 46 ------------------- .github/workflows/iron-pre-commit.yml | 13 ------ .github/workflows/iron-semi-binary-build.yml | 48 -------------------- README.md | 1 - ros2_control_demos-not-released.iron.repos | 1 - ros2_control_demos.iron.repos | 21 --------- 10 files changed, 211 deletions(-) delete mode 100644 .github/workflows/iron-binary-build.yml delete mode 100644 .github/workflows/iron-check-docs.yml delete mode 100644 .github/workflows/iron-docker-build.yaml delete mode 100644 .github/workflows/iron-pre-commit.yml delete mode 100644 .github/workflows/iron-semi-binary-build.yml delete mode 100644 ros2_control_demos-not-released.iron.repos delete mode 100644 ros2_control_demos.iron.repos diff --git a/.github/dependabot.yml b/.github/dependabot.yml index 766bb548..f5e9921f 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -11,13 +11,6 @@ updates: directory: "/" schedule: interval: "weekly" - - package-ecosystem: "github-actions" - # Workflow files stored in the - # default location of `.github/workflows` - directory: "/" - schedule: - interval: "weekly" - target-branch: "iron" - package-ecosystem: "github-actions" # Workflow files stored in the # default location of `.github/workflows` diff --git a/.github/mergify.yml b/.github/mergify.yml index fd185e02..84ad380e 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,15 +8,6 @@ pull_request_rules: branches: - humble - - name: Backport to iron at reviewers discretion - conditions: - - base=master - - "label=backport-iron" - actions: - backport: - branches: - - iron - - name: Ask to resolve conflict conditions: - conflict diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml deleted file mode 100644 index 6fdc7600..00000000 --- a/.github/workflows/iron-binary-build.yml +++ /dev/null @@ -1,48 +0,0 @@ -name: Iron Binary Build -# author: Denis Štogl -# description: 'Build & test all dependencies from released (binary) packages.' - -on: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.cpp' - - '.github/workflows/iron-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - '**.xacro' - - '**.py' - - '**.yaml' - - 'ros2_control_demos-not-released.iron.repos' - push: - branches: - - iron - paths: - - '**.hpp' - - '**.cpp' - - '.github/workflows/iron-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - '**.xacro' - - '**.py' - - '**.yaml' - - 'ros2_control_demos-not-released.iron.repos' - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '03 1 * * *' - -jobs: - binary: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [iron] - ROS_REPO: [main, testing] - with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_control_demos-not-released.${{ matrix.ROS_DISTRO }}.repos - ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-check-docs.yml b/.github/workflows/iron-check-docs.yml deleted file mode 100644 index cb3b2c06..00000000 --- a/.github/workflows/iron-check-docs.yml +++ /dev/null @@ -1,17 +0,0 @@ -name: Iron Check Docs - -on: - workflow_dispatch: - pull_request: - branches: - - iron - paths: - - '**.rst' - - '.github/workflows/iron-check-docs.yml' - -jobs: - check-docs: - name: Check Docs - uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@iron - with: - ROS2_CONTROL_DEMOS_PR: ${{ github.ref }} diff --git a/.github/workflows/iron-docker-build.yaml b/.github/workflows/iron-docker-build.yaml deleted file mode 100644 index 03f91c40..00000000 --- a/.github/workflows/iron-docker-build.yaml +++ /dev/null @@ -1,46 +0,0 @@ -name: Build Iron Dockerfile -# description: builds the dockerfile contained within the repo - -on: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.cpp' - - Dockerfile/** - - '.github/workflows/iron-docker-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - '**.xacro' - - '**.py' - - '**.yaml' - - 'ros2_control_demos.iron.repos' - push: - branches: - - iron - paths: - - '**.hpp' - - '**.cpp' - - Dockerfile/** - - '.github/workflows/iron-docker-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - '**.xacro' - - '**.py' - - '**.yaml' - - 'ros2_control_demos.iron.repos' - schedule: - # Run every morning to detect broken dependencies - - cron: '50 1 * * *' - - -jobs: - build: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - with: - ref: iron - - name: Build the Docker image - run: docker build --file Dockerfile/Dockerfile --tag ros2_control_demos_iron --build-arg ROS_DISTRO=iron . diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml deleted file mode 100644 index a1289580..00000000 --- a/.github/workflows/iron-pre-commit.yml +++ /dev/null @@ -1,13 +0,0 @@ -name: Pre-Commit - Iron - -on: - workflow_dispatch: - pull_request: - branches: - - iron - -jobs: - pre-commit: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master - with: - ros_distro: iron diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml deleted file mode 100644 index 400bc327..00000000 --- a/.github/workflows/iron-semi-binary-build.yml +++ /dev/null @@ -1,48 +0,0 @@ -name: Iron Semi-Binary Build -# description: 'Build & test that compiles the main dependencies from source.' - -on: - pull_request: - branches: - - iron - paths: - - '**.hpp' - - '**.cpp' - - '.github/workflows/iron-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - '**.xacro' - - '**.py' - - '**.yaml' - - 'ros2_control_demos.iron.repos' - push: - branches: - - iron - paths: - - '**.hpp' - - '**.cpp' - - '.github/workflows/iron-semi-binary-build.yml' - - '**/package.xml' - - '**/CMakeLists.txt' - - '**.xacro' - - '**.py' - - '**.yaml' - - 'ros2_control_demos.iron.repos' - - schedule: - # Run every morning to detect flakiness and broken dependencies - - cron: '33 1 * * *' - -jobs: - semi_binary: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [iron] - ROS_REPO: [testing] - with: - ros_distro: ${{ matrix.ROS_DISTRO }} - ros_repo: ${{ matrix.ROS_REPO }} - upstream_workspace: ros2_control_demos.${{ matrix.ROS_DISTRO }}.repos - ref_for_scheduled_build: iron diff --git a/README.md b/README.md index ea154320..fa7a539c 100644 --- a/README.md +++ b/README.md @@ -112,7 +112,6 @@ ROS 2 Distro | Branch | Build status | Documentation :----------: | :----: | :----------: | :-----------: **Rolling** | [`master`](https://github.com/ros-controls/ros2_control_demos/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build.yml?branch=master)
| [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) **Jazzy** | [`master`](https://github.com/ros-controls/ros2_control_demos/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build.yml?branch=master)
| [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) -**Iron** | [`iron`](https://github.com/ros-controls/ros2_control_demos/tree/iron) | [![Iron Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/iron-binary-build.yml/badge.svg?branch=iron)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/iron-binary-build.yml?branch=iron)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/iron-semi-binary-build.yml/badge.svg?branch=iron)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/iron-semi-binary-build.yml?branch=iron)
| [Documentation](https://control.ros.org/iron/index.html)
[API Reference](https://control.ros.org/iron/doc/api/index.html) **Humble** | [`humble`](https://github.com/ros-controls/ros2_control_demos/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/humble-binary-build.yml/badge.svg?branch=humble)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/humble-binary-build.yml?branch=humble)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/humble-semi-binary-build.yml/badge.svg?branch=humble)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/humble-semi-binary-build.yml?branch=humble)
| [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) diff --git a/ros2_control_demos-not-released.iron.repos b/ros2_control_demos-not-released.iron.repos deleted file mode 100644 index 56f46b6f..00000000 --- a/ros2_control_demos-not-released.iron.repos +++ /dev/null @@ -1 +0,0 @@ -repositories: diff --git a/ros2_control_demos.iron.repos b/ros2_control_demos.iron.repos deleted file mode 100644 index 29d4ba16..00000000 --- a/ros2_control_demos.iron.repos +++ /dev/null @@ -1,21 +0,0 @@ -repositories: - control_msgs: - type: git - url: https://github.com/ros-controls/control_msgs.git - version: master - realtime_tools: - type: git - url: https://github.com/ros-controls/realtime_tools.git - version: master - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control.git - version: iron - ros2_controllers: - type: git - url: https://github.com/ros-controls/ros2_controllers.git - version: iron - gazebo_ros2_control: - type: git - url: https://github.com/ros-controls/gazebo_ros2_control.git - version: iron From 34629b57000e34588b2e0ab800fb396fb43a7cdc Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 1 Dec 2024 11:11:50 +0100 Subject: [PATCH 6/7] Bump version of pre-commit hooks (#655) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 12546c1a..1c3b460d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -62,7 +62,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.3 + rev: v19.1.4 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -131,7 +131,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.4 + rev: 0.30.0 hooks: - id: check-github-workflows args: ["--verbose"] From 75a6e0c716ac2bc3c699def461987ce062420844 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 4 Dec 2024 17:44:31 +0100 Subject: [PATCH 7/7] Change interface export variant of all examples (#623) --- .../ros2_control_demo_example_10/rrbot.hpp | 13 +- example_10/hardware/rrbot.cpp | 106 ++++++--------- example_11/hardware/carlikebot_system.cpp | 118 ++++++----------- .../carlikebot_system.hpp | 11 +- .../ros2_control_demo_example_12/rrbot.hpp | 8 -- example_12/hardware/rrbot.cpp | 54 ++------ example_2/hardware/diffbot_system.cpp | 80 ++++++----- .../diffbot_system.hpp | 10 +- .../rrbot_system_multi_interface.hpp | 13 +- .../hardware/rrbot_system_multi_interface.cpp | 125 +++++++----------- .../rrbot_system_with_sensor.hpp | 9 -- .../hardware/rrbot_system_with_sensor.cpp | 90 ++++--------- .../external_rrbot_force_torque_sensor.cpp | 34 +---- .../external_rrbot_force_torque_sensor.hpp | 5 - .../ros2_control_demo_example_5/rrbot.hpp | 8 -- example_5/hardware/rrbot.cpp | 54 ++------ .../rrbot_actuator.hpp | 9 +- example_6/hardware/rrbot_actuator.cpp | 67 ++++++---- example_7/doc/userdoc.rst | 37 ++---- .../r6bot_hardware.hpp | 14 +- example_7/hardware/r6bot_hardware.cpp | 88 +++--------- .../reference_generator/send_trajectory.cpp | 9 ++ ...bot_transmissions_system_position_only.hpp | 4 - ...bot_transmissions_system_position_only.cpp | 67 +++++----- .../ros2_control_demo_example_9/rrbot.hpp | 8 -- example_9/hardware/rrbot.cpp | 54 ++------ 26 files changed, 365 insertions(+), 730 deletions(-) diff --git a/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp b/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp index 11bc5e1e..f3e67f79 100644 --- a/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp +++ b/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp @@ -40,10 +40,6 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -58,12 +54,9 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface private: // Parameters for the RRBot simulation - - // Store the command and state interfaces for the simulated robot - std::vector hw_commands_; - std::vector hw_states_; - std::vector hw_gpio_in_; - std::vector hw_gpio_out_; + double hw_start_sec_; + double hw_stop_sec_; + double hw_slowdown_; }; } // namespace ros2_control_demo_example_10 diff --git a/example_10/hardware/rrbot.cpp b/example_10/hardware/rrbot.cpp index 9c0d2836..4e76f137 100644 --- a/example_10/hardware/rrbot.cpp +++ b/example_10/hardware/rrbot.cpp @@ -37,9 +37,6 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - for (const hardware_interface::ComponentInfo & joint : info_.joints) { // RRBotSystemPositionOnly has exactly one state and command interface on each joint @@ -123,67 +120,25 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_configure( // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - std::fill(hw_states_.begin(), hw_states_.end(), 0); - std::fill(hw_commands_.begin(), hw_commands_.end(), 0); - std::fill(hw_gpio_in_.begin(), hw_gpio_in_.end(), 0); - std::fill(hw_gpio_out_.begin(), hw_gpio_out_.end(), 0); - - RCLCPP_INFO(get_logger(), "Successfully configured!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSystemWithGPIOHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); + set_state(name, 0.0); } - - RCLCPP_INFO(get_logger(), "State interfaces:"); - hw_gpio_in_.resize(4); - size_t ct = 0; - for (size_t i = 0; i < info_.gpios.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { - for (auto state_if : info_.gpios.at(i).state_interfaces) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.gpios.at(i).name, state_if.name, &hw_gpio_in_[ct++])); - RCLCPP_INFO( - get_logger(), "Added %s/%s", info_.gpios.at(i).name.c_str(), state_if.name.c_str()); - } + set_command(name, 0.0); } - - return state_interfaces; -} - -std::vector -RRBotSystemWithGPIOHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) + for (const auto & [name, descr] : gpio_state_interfaces_) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + set_state(name, 0.0); } - RCLCPP_INFO(get_logger(), "Command interfaces:"); - hw_gpio_out_.resize(2); - size_t ct = 0; - for (size_t i = 0; i < info_.gpios.size(); i++) + for (const auto & [name, descr] : gpio_command_interfaces_) { - for (auto command_if : info_.gpios.at(i).command_interfaces) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.gpios.at(i).name, command_if.name, &hw_gpio_out_[ct++])); - RCLCPP_INFO( - get_logger(), "Added %s/%s", info_.gpios.at(i).name.c_str(), command_if.name.c_str()); - } + set_command(name, 0.0); } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate( @@ -194,9 +149,13 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_command(name, get_state(name)); + } + for (const auto & [name, descr] : gpio_command_interfaces_) { - hw_commands_[i] = hw_states_[i]; + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -221,25 +180,33 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::read( std::stringstream ss; ss << "Reading states:"; - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]); + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); + } + + for (const auto & [name, descr] : gpio_command_interfaces_) + { + // mirror GPIOs back + set_state(name, get_command(name)); } - // mirror GPIOs back - hw_gpio_in_[0] = hw_gpio_out_[0]; - hw_gpio_in_[3] = hw_gpio_out_[1]; - // random inputs + // random inputs analog_input1 and analog_input2 unsigned int seed = time(NULL) + 1; - hw_gpio_in_[1] = static_cast(rand_r(&seed)); + set_state( + info_.gpios[0].name + "/" + info_.gpios[0].state_interfaces[1].name, + static_cast(rand_r(&seed))); seed = time(NULL) + 2; - hw_gpio_in_[2] = static_cast(rand_r(&seed)); + set_state( + info_.gpios[0].name + "/" + info_.gpios[0].state_interfaces[2].name, + static_cast(rand_r(&seed))); - for (uint i = 0; i < hw_gpio_in_.size(); i++) + for (const auto & [name, descr] : gpio_state_interfaces_) { ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_gpio_in_[i] << " from GPIO input '" << static_cast(i) << "'"; + << "\t" << get_state(name) << " from GPIO input '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -254,10 +221,11 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::write( std::stringstream ss; ss << "Writing commands:"; - for (uint i = 0; i < hw_gpio_out_.size(); i++) + for (const auto & [name, descr] : gpio_command_interfaces_) { + // Simulate sending commands to the hardware ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_gpio_out_[i] << " for GPIO output '" << static_cast(i) << "'"; + << "\t" << get_command(name) << " for GPIO output '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index b8deca75..a1100c0d 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -56,6 +56,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( // Steering joints have a position command interface and a position state interface if (joint_is_steering) { + steering_joint_ = joint.name; RCLCPP_INFO(get_logger(), "Joint '%s' is a steering joint.", joint.name.c_str()); if (joint.command_interfaces.size() != 1) @@ -93,6 +94,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( else { RCLCPP_INFO(get_logger(), "Joint '%s' is a drive joint.", joint.name.c_str()); + traction_joint_ = joint.name; // Drive joints have a velocity command interface and a velocity state interface if (joint.command_interfaces.size() != 1) @@ -143,69 +145,33 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); // // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_interfaces_["steering"] = Joint("virtual_front_wheel_joint"); - - hw_interfaces_["traction"] = Joint("virtual_rear_wheel_joint"); - return hardware_interface::CallbackReturn::SUCCESS; } -std::vector CarlikeBotSystemHardware::export_state_interfaces() +hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { - std::vector state_interfaces; + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); - for (auto & joint : hw_interfaces_) + for (auto i = 0; i < hw_start_sec_; i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint.second.joint_name, hardware_interface::HW_IF_POSITION, &joint.second.state.position)); - - if (joint.first == "traction") - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, &joint.second.state.velocity)); - } + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } - RCLCPP_INFO(get_logger(), "Exported %zu state interfaces.", state_interfaces.size()); - - for (auto s : state_interfaces) + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) { - RCLCPP_INFO(get_logger(), "Exported state interface '%s'.", s.get_name().c_str()); + set_state(name, 0.0); } - - return state_interfaces; -} - -std::vector -CarlikeBotSystemHardware::export_command_interfaces() -{ - std::vector command_interfaces; - - for (auto & joint : hw_interfaces_) + for (const auto & [name, descr] : joint_command_interfaces_) { - if (joint.first == "steering") - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joint.second.joint_name, hardware_interface::HW_IF_POSITION, - &joint.second.command.position)); - } - else if (joint.first == "traction") - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, - &joint.second.command.velocity)); - } + set_command(name, 0.0); } - RCLCPP_INFO(get_logger(), "Exported %zu command interfaces.", command_interfaces.size()); - - for (auto i = 0u; i < command_interfaces.size(); i++) - { - RCLCPP_INFO( - get_logger(), "Exported command interface '%s'.", command_interfaces[i].get_name().c_str()); - } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( @@ -219,20 +185,10 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } - for (auto & joint : hw_interfaces_) + // command and state should be equal when starting + for (const auto & [name, descr] : joint_command_interfaces_) { - joint.second.state.position = 0.0; - - if (joint.first == "traction") - { - joint.second.state.velocity = 0.0; - joint.second.command.velocity = 0.0; - } - - else if (joint.first == "steering") - { - joint.second.command.position = 0.0; - } + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -261,26 +217,32 @@ hardware_interface::return_type CarlikeBotSystemHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - - hw_interfaces_["steering"].state.position = hw_interfaces_["steering"].command.position; - - hw_interfaces_["traction"].state.velocity = hw_interfaces_["traction"].command.velocity; - hw_interfaces_["traction"].state.position += - hw_interfaces_["traction"].state.velocity * period.seconds(); + // update states from commands and integrate velocity to position + set_state( + steering_joint_ + "/" + hardware_interface::HW_IF_POSITION, + get_command(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION)); + + set_state( + traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY, + get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY)); + set_state( + traction_joint_ + "/" + hardware_interface::HW_IF_POSITION, + get_state(traction_joint_ + "/" + hardware_interface::HW_IF_POSITION) + + get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY) * period.seconds()); std::stringstream ss; ss << "Reading states:"; ss << std::fixed << std::setprecision(2) << std::endl << "\t" - << "position: " << hw_interfaces_["steering"].state.position << " for joint '" - << hw_interfaces_["steering"].joint_name.c_str() << "'" << std::endl + << "position: " << get_state(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION) + << " for joint '" << steering_joint_ << "'" << std::endl << "\t" - << "position: " << hw_interfaces_["traction"].state.position << " for joint '" - << hw_interfaces_["traction"].joint_name.c_str() << "'" << std::endl + << "position: " << get_state(traction_joint_ + "/" + hardware_interface::HW_IF_POSITION) + << " for joint '" << traction_joint_ << "'" << std::endl << "\t" - << "velocity: " << hw_interfaces_["traction"].state.velocity << " for joint '" - << hw_interfaces_["traction"].joint_name.c_str() << "'"; + << "velocity: " << get_state(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY) + << " for joint '" << traction_joint_ << "'"; RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); @@ -298,11 +260,11 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH ss << std::fixed << std::setprecision(2) << std::endl << "\t" - << "position: " << hw_interfaces_["steering"].command.position << " for joint '" - << hw_interfaces_["steering"].joint_name.c_str() << "'" << std::endl + << "position: " << get_command(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION) + << " for joint '" << steering_joint_ << "'" << std::endl << "\t" - << "velocity: " << hw_interfaces_["traction"].command.velocity << " for joint '" - << hw_interfaces_["traction"].joint_name.c_str() << "'"; + << "velocity: " << get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY) + << " for joint '" << traction_joint_ << "'"; RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 44ab8a1f..e92c40c4 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -63,9 +63,8 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -84,9 +83,9 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface double hw_start_sec_; double hw_stop_sec_; - // std::vector> - // hw_interfaces_; // name of joint, state, command - std::map hw_interfaces_; + // joint names + std::string steering_joint_; + std::string traction_joint_; }; } // namespace ros2_control_demo_example_11 diff --git a/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp b/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp index ac116044..47064b8a 100644 --- a/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp +++ b/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp @@ -40,10 +40,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -61,10 +57,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; - - // Store the command for the simulated robot - std::vector hw_commands_; - std::vector hw_states_; }; } // namespace ros2_control_demo_example_12 diff --git a/example_12/hardware/rrbot.cpp b/example_12/hardware/rrbot.cpp index 53e20ab7..a6cc4cf7 100644 --- a/example_12/hardware/rrbot.cpp +++ b/example_12/hardware/rrbot.cpp @@ -43,8 +43,6 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); for (const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -100,41 +98,17 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { - hw_states_[i] = 0; - hw_commands_[i] = 0; + set_state(name, 0.0); } - - RCLCPP_INFO(get_logger(), "Successfully configured!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSystemPositionOnlyHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); - } - - return state_interfaces; -} - -std::vector -RRBotSystemPositionOnlyHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + set_command(name, 0.0); } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( @@ -151,9 +125,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { - hw_commands_[i] = hw_states_[i]; + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -186,13 +160,13 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( std::stringstream ss; ss << "Reading states:"; - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; - + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name.c_str() << "'"; + << "\t" << get_state(name) << " for joint '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -207,11 +181,11 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( std::stringstream ss; ss << "Writing commands:"; - for (uint i = 0; i < hw_commands_.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { // Simulate sending commands to the hardware ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name.c_str() << "'"; + << "\t" << get_command(name) << " for joint '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_2/hardware/diffbot_system.cpp b/example_2/hardware/diffbot_system.cpp index 87a21f53..28574f1f 100644 --- a/example_2/hardware/diffbot_system.cpp +++ b/example_2/hardware/diffbot_system.cpp @@ -45,9 +45,6 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( hw_stop_sec_ = hardware_interface::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); for (const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -99,30 +96,31 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector DiffBotSystemHardware::export_state_interfaces() +hardware_interface::CallbackReturn DiffBotSystemHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { - std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } + // END: This part here is for exemplary purposes - Please do not copy to your production code - return state_interfaces; -} - -std::vector DiffBotSystemHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + set_state(name, 0.0); } + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, 0.0); + } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( @@ -138,15 +136,10 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( } // END: This part here is for exemplary purposes - Please do not copy to your production code - // set some default values - for (auto i = 0u; i < hw_positions_.size(); i++) + // command and state should be equal when starting + for (const auto & [name, descr] : joint_command_interfaces_) { - if (std::isnan(hw_positions_[i])) - { - hw_positions_[i] = 0; - hw_velocities_[i] = 0; - hw_commands_[i] = 0; - } + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -178,18 +171,21 @@ hardware_interface::return_type DiffBotSystemHardware::read( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; ss << "Reading states:"; - for (std::size_t i = 0; i < hw_velocities_.size(); i++) + ss << std::fixed << std::setprecision(2); + for (const auto & [name, descr] : joint_state_interfaces_) { - // Simulate DiffBot wheels's movement as a first-order system - // Update the joint status: this is a revolute joint without any limit. - // Simply integrates - hw_positions_[i] = hw_positions_[i] + period.seconds() * hw_velocities_[i]; - - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" - "position " - << hw_positions_[i] << " and velocity " << hw_velocities_[i] << " for '" - << info_.joints[i].name.c_str() << "'!"; + if (descr.get_interface_name() == hardware_interface::HW_IF_POSITION) + { + // Simulate DiffBot wheels's movement as a first-order system + // Update the joint status: this is a revolute joint without any limit. + // Simply integrates + auto velo = get_command(descr.get_prefix_name() + "/" + hardware_interface::HW_IF_VELOCITY); + set_state(name, get_state(name) + period.seconds() * velo); + + ss << std::endl + << "\t position " << get_state(name) << " and velocity " << velo << " for '" << name + << "'!"; + } } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -203,13 +199,13 @@ hardware_interface::return_type ros2_control_demo_example_2 ::DiffBotSystemHardw // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; ss << "Writing commands:"; - for (auto i = 0u; i < hw_commands_.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { // Simulate sending commands to the hardware - hw_velocities_[i] = hw_commands_[i]; + set_state(name, get_command(name)); ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << "command " << hw_commands_[i] << " for '" << info_.joints[i].name.c_str() << "'!"; + << "\t" << "command " << get_command(name) << " for '" << name << "'!"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp b/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp index 1e383abd..bf5bd968 100644 --- a/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp +++ b/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp @@ -40,9 +40,8 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -60,11 +59,6 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface // Parameters for the DiffBot simulation double hw_start_sec_; double hw_stop_sec_; - - // Store the command for the simulated robot - std::vector hw_commands_; - std::vector hw_positions_; - std::vector hw_velocities_; }; } // namespace ros2_control_demo_example_2 diff --git a/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp b/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp index 7469de7c..a952e077 100644 --- a/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp +++ b/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp @@ -42,9 +42,8 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; hardware_interface::return_type prepare_command_mode_switch( const std::vector & start_interfaces, @@ -68,14 +67,6 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter double hw_stop_sec_; double hw_slowdown_; - // Store the commands for the simulated robot - std::vector hw_commands_positions_; - std::vector hw_commands_velocities_; - std::vector hw_commands_accelerations_; - std::vector hw_states_positions_; - std::vector hw_states_velocities_; - std::vector hw_states_accelerations_; - // Enum defining at which control level we are // Dumb way of maintaining the command_interface type per joint. enum integration_level_t : std::uint8_t diff --git a/example_3/hardware/rrbot_system_multi_interface.cpp b/example_3/hardware/rrbot_system_multi_interface.cpp index 0818056a..213c17b3 100644 --- a/example_3/hardware/rrbot_system_multi_interface.cpp +++ b/example_3/hardware/rrbot_system_multi_interface.cpp @@ -43,12 +43,6 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_states_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_states_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_states_accelerations_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_accelerations_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); control_level_.resize(info_.joints.size(), integration_level_t::POSITION); for (const hardware_interface::ComponentInfo & joint : info_.joints) @@ -99,39 +93,31 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -RRBotSystemMultiInterfaceHardware::export_state_interfaces() +hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { - std::vector state_interfaces; - for (std::size_t i = 0; i < info_.joints.size(); i++) + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_positions_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocities_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &hw_states_accelerations_[i])); + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } + // END: This part here is for exemplary purposes - Please do not copy to your production code - return state_interfaces; -} - -std::vector -RRBotSystemMultiInterfaceHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (std::size_t i = 0; i < info_.joints.size(); i++) + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_positions_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_velocities_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, - &hw_commands_accelerations_[i])); + set_state(name, 0.0); } + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, 0.0); + } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::return_type RRBotSystemMultiInterfaceHardware::prepare_command_mode_switch( @@ -178,8 +164,11 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::prepare_comma { if (key.find(info_.joints[i].name) != std::string::npos) { - hw_commands_velocities_[i] = 0; - hw_commands_accelerations_[i] = 0; + set_command( + info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, + get_state(info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION)); + set_command(info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, 0.0); + set_command(info_.joints[i].name + "/" + hardware_interface::HW_IF_ACCELERATION, 0.0); control_level_[i] = integration_level_t::UNDEFINED; // Revert to undefined } } @@ -211,32 +200,8 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_activat // END: This part here is for exemplary purposes - Please do not copy to your production code // Set some default values - for (std::size_t i = 0; i < hw_states_positions_.size(); i++) + for (std::size_t i = 0; i < info_.joints.size(); i++) { - if (std::isnan(hw_states_positions_[i])) - { - hw_states_positions_[i] = 0; - } - if (std::isnan(hw_states_velocities_[i])) - { - hw_states_velocities_[i] = 0; - } - if (std::isnan(hw_states_accelerations_[i])) - { - hw_states_accelerations_[i] = 0; - } - if (std::isnan(hw_commands_positions_[i])) - { - hw_commands_positions_[i] = 0; - } - if (std::isnan(hw_commands_velocities_[i])) - { - hw_commands_velocities_[i] = 0; - } - if (std::isnan(hw_commands_accelerations_[i])) - { - hw_commands_accelerations_[i] = 0; - } control_level_[i] = integration_level_t::UNDEFINED; } @@ -268,8 +233,11 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; ss << "Reading states:"; - for (std::size_t i = 0; i < hw_states_positions_.size(); i++) + for (std::size_t i = 0; i < info_.joints.size(); i++) { + const auto name_acc = info_.joints[i].name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto name_vel = info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY; + const auto name_pos = info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION; switch (control_level_[i]) { case integration_level_t::UNDEFINED: @@ -277,26 +245,30 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read( return hardware_interface::return_type::OK; break; case integration_level_t::POSITION: - hw_states_accelerations_[i] = 0; - hw_states_velocities_[i] = 0; - hw_states_positions_[i] += - (hw_commands_positions_[i] - hw_states_positions_[i]) / hw_slowdown_; + set_state(name_acc, 0.); + set_state(name_vel, 0.); + set_state( + name_pos, + get_state(name_pos) + (get_command(name_pos) - get_state(name_pos)) / hw_slowdown_); break; case integration_level_t::VELOCITY: - hw_states_accelerations_[i] = 0; - hw_states_velocities_[i] = hw_commands_velocities_[i]; - hw_states_positions_[i] += (hw_states_velocities_[i] * period.seconds()) / hw_slowdown_; + set_state(name_acc, 0.); + set_state(name_vel, get_command(name_vel)); + set_state( + name_pos, get_state(name_pos) + get_state(name_vel) * period.seconds() / hw_slowdown_); break; case integration_level_t::ACCELERATION: - hw_states_accelerations_[i] = hw_commands_accelerations_[i]; - hw_states_velocities_[i] += (hw_states_accelerations_[i] * period.seconds()) / hw_slowdown_; - hw_states_positions_[i] += (hw_states_velocities_[i] * period.seconds()) / hw_slowdown_; + set_state(name_acc, get_command(name_acc)); + set_state( + name_vel, get_state(name_vel) + get_state(name_acc) * period.seconds() / hw_slowdown_); + set_state( + name_pos, get_state(name_pos) + get_state(name_vel) * period.seconds() / hw_slowdown_); break; } ss << std::fixed << std::setprecision(2) << std::endl << "\t" - << "pos: " << hw_states_positions_[i] << ", vel: " << hw_states_velocities_[i] - << ", acc: " << hw_states_accelerations_[i] << " for joint " << i; + << "pos: " << get_state(name_pos) << ", vel: " << get_state(name_vel) + << ", acc: " << get_state(name_acc) << " for joint " << i; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -309,13 +281,16 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::write( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; ss << "Writing commands:"; - for (std::size_t i = 0; i < hw_commands_positions_.size(); i++) + for (std::size_t i = 0; i < info_.joints.size(); i++) { // Simulate sending commands to the hardware + const auto name_acc = info_.joints[i].name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto name_vel = info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY; + const auto name_pos = info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION; ss << std::fixed << std::setprecision(2) << std::endl << "\t" - << "command pos: " << hw_commands_positions_[i] << ", vel: " << hw_commands_velocities_[i] - << ", acc: " << hw_commands_accelerations_[i] << " for joint " << i + << "command pos: " << get_command(name_pos) << ", vel: " << get_command(name_vel) + << ", acc: " << get_command(name_acc) << " for joint " << i << ", control lvl: " << static_cast(control_level_[i]); } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); diff --git a/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp b/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp index e166c990..f09fcf3c 100644 --- a/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp +++ b/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp @@ -43,10 +43,6 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -65,11 +61,6 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface double hw_stop_sec_; double hw_slowdown_; double hw_sensor_change_; - - // Store the command for the simulated robot - std::vector hw_joint_commands_; - std::vector hw_joint_states_; - std::vector hw_sensor_states_; }; } // namespace ros2_control_demo_example_4 diff --git a/example_4/hardware/rrbot_system_with_sensor.cpp b/example_4/hardware/rrbot_system_with_sensor.cpp index 751cbc51..f7df7eb8 100644 --- a/example_4/hardware/rrbot_system_with_sensor.cpp +++ b/example_4/hardware/rrbot_system_with_sensor.cpp @@ -48,11 +48,6 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_init( hw_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_sensor_states_.resize( - info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); - for (const hardware_interface::ComponentInfo & joint : info_.joints) { // RRBotSystemWithSensor has exactly one state and command interface on each joint @@ -107,48 +102,21 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_configure( // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - for (uint i = 0; i < hw_joint_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { - hw_joint_states_[i] = 0; - hw_joint_commands_[i] = 0; + set_state(name, 0.0); } - - RCLCPP_INFO(get_logger(), "Successfully configured!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSystemWithSensorHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) + for (const auto & [name, descr] : sensor_state_interfaces_) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_joint_states_[i])); + set_state(name, 0.0); } - - // export sensor state interface - for (uint i = 0; i < info_.sensors[0].state_interfaces.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &hw_sensor_states_[i])); - } - - return state_interfaces; -} - -std::vector -RRBotSystemWithSensorHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_joint_commands_[i])); + set_command(name, 0.0); } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( @@ -165,17 +133,10 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_joint_states_.size(); i++) - { - hw_joint_commands_[i] = hw_joint_states_[i]; - } - - // set default value for sensor - if (std::isnan(hw_sensor_states_[0])) + for (const auto & [name, descr] : joint_command_interfaces_) { - hw_sensor_states_[0] = 0; + set_command(name, get_state(name)); } - RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; @@ -204,30 +165,28 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read( { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; - ss << "Reading states from joints:"; + ss << "Reading states from joints:" << std::fixed << std::setprecision(2); - for (uint i = 0; i < hw_joint_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_joint_states_[i] += (hw_joint_commands_[i] - hw_joint_states_[i]) / hw_slowdown_; - - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_joint_states_[i] << " for joint '" << info_.joints[i].name.c_str() << "'"; + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); + ss << std::endl << "\t" << get_state(name) << " for joint '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); ss.str(""); - ss << "Reading states from sensors:"; - for (uint i = 0; i < hw_sensor_states_.size(); i++) + ss << "Reading states from sensors:" << std::fixed << std::setprecision(2); + size_t i = 0; + for (const auto & [name, descr] : sensor_state_interfaces_) { // Simulate RRBot's sensor data - unsigned int seed = time(NULL) + i; - hw_sensor_states_[i] = - static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_)); + unsigned int seed = time(NULL) + i++; + set_state( + name, static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_))); - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_sensor_states_[i] << " for sensor '" - << info_.sensors[0].state_interfaces[i].name.c_str() << "'"; + ss << std::endl << "\t" << get_state(name) << " for sensor '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -241,12 +200,11 @@ hardware_interface::return_type ros2_control_demo_example_4::RRBotSystemWithSens // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; ss << "Writing commands:"; - - for (uint i = 0; i < hw_joint_commands_.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { // Simulate sending commands to the hardware ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_joint_commands_[i] << " for joint '" << info_.joints[i].name.c_str() << "'"; + << "\t" << get_command(name) << " for joint '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_5/hardware/external_rrbot_force_torque_sensor.cpp b/example_5/hardware/external_rrbot_force_torque_sensor.cpp index aadc299c..11c76ad3 100644 --- a/example_5/hardware/external_rrbot_force_torque_sensor.cpp +++ b/example_5/hardware/external_rrbot_force_torque_sensor.cpp @@ -47,27 +47,9 @@ hardware_interface::CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_in hw_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_sensor_states_.resize( - info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); - return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -ExternalRRBotForceTorqueSensorHardware::export_state_interfaces() -{ - std::vector state_interfaces; - - // export sensor state interface - for (uint i = 0; i < info_.sensors[0].state_interfaces.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &hw_sensor_states_[i])); - } - - return state_interfaces; -} - hardware_interface::CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -109,18 +91,16 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read( { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; - ss << "Reading states:"; - - for (uint i = 0; i < hw_sensor_states_.size(); i++) + ss << "Reading states from sensors:" << std::fixed << std::setprecision(2); + size_t i = 0; + for (const auto & [name, descr] : sensor_state_interfaces_) { // Simulate RRBot's sensor data - unsigned int seed = time(NULL) + i; - hw_sensor_states_[i] = - static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_)); + unsigned int seed = time(NULL) + i++; + set_state( + name, static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_))); - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_sensor_states_[i] << " for sensor '" - << info_.sensors[0].state_interfaces[i].name.c_str() << "'"; + ss << std::endl << "\t" << get_state(name) << " for sensor '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp b/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp index 3f0a3a94..d6426544 100644 --- a/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp +++ b/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp @@ -39,8 +39,6 @@ class ExternalRRBotForceTorqueSensorHardware : public hardware_interface::Sensor hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -55,9 +53,6 @@ class ExternalRRBotForceTorqueSensorHardware : public hardware_interface::Sensor double hw_start_sec_; double hw_stop_sec_; double hw_sensor_change_; - - // Store the sensor states for the simulated robot - std::vector hw_sensor_states_; }; } // namespace ros2_control_demo_example_5 diff --git a/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp b/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp index 3fdac8de..b1961a1f 100644 --- a/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp +++ b/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp @@ -40,10 +40,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -61,10 +57,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; - - // Store the command for the simulated robot - std::vector hw_commands_; - std::vector hw_states_; }; } // namespace ros2_control_demo_example_5 diff --git a/example_5/hardware/rrbot.cpp b/example_5/hardware/rrbot.cpp index 6c395451..eb2d645c 100644 --- a/example_5/hardware/rrbot.cpp +++ b/example_5/hardware/rrbot.cpp @@ -43,8 +43,6 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); for (const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -100,41 +98,17 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { - hw_states_[i] = 0; - hw_commands_[i] = 0; + set_state(name, 0.0); } - - RCLCPP_INFO(get_logger(), "Successfully configured!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSystemPositionOnlyHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); - } - - return state_interfaces; -} - -std::vector -RRBotSystemPositionOnlyHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + set_command(name, 0.0); } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( @@ -151,9 +125,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { - hw_commands_[i] = hw_states_[i]; + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -186,13 +160,13 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( std::stringstream ss; ss << "Reading states:"; - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; - + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name.c_str() << "'"; + << "\t" << get_state(name) << " for joint '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -207,11 +181,11 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( std::stringstream ss; ss << "Writing commands:"; - for (uint i = 0; i < hw_commands_.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { // Simulate sending commands to the hardware ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name.c_str() << "'"; + << "\t" << get_command(name) << " for joint '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp b/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp index 4874bd47..f650989d 100644 --- a/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp +++ b/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp @@ -40,9 +40,8 @@ class RRBotModularJoint : public hardware_interface::ActuatorInterface hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -61,10 +60,6 @@ class RRBotModularJoint : public hardware_interface::ActuatorInterface double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; - - // Store the command for the simulated robot - double hw_joint_command_; - double hw_joint_state_; }; } // namespace ros2_control_demo_example_6 diff --git a/example_6/hardware/rrbot_actuator.cpp b/example_6/hardware/rrbot_actuator.cpp index a1528b62..0df4c778 100644 --- a/example_6/hardware/rrbot_actuator.cpp +++ b/example_6/hardware/rrbot_actuator.cpp @@ -47,9 +47,6 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_init( hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_joint_state_ = std::numeric_limits::quiet_NaN(); - hw_joint_command_ = std::numeric_limits::quiet_NaN(); - const hardware_interface::ComponentInfo & joint = info_.joints[0]; // RRBotModularJoint has exactly one state and command interface on each joint if (joint.command_interfaces.size() != 1) @@ -88,24 +85,31 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_init( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector RRBotModularJoint::export_state_interfaces() +hardware_interface::CallbackReturn RRBotModularJoint::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { - std::vector state_interfaces; - - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_state_)); - - return state_interfaces; -} + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); -std::vector RRBotModularJoint::export_command_interfaces() -{ - std::vector command_interfaces; + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_command_)); + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_state(name, 0.0); + } + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, 0.0); + } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotModularJoint::on_activate( @@ -121,11 +125,10 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_activate( } // END: This part here is for exemplary purposes - Please do not copy to your production code - // set some default values for joints - if (std::isnan(hw_joint_state_)) + // command and state should be equal when starting + for (const auto & [name, descr] : joint_state_interfaces_) { - hw_joint_state_ = 0; - hw_joint_command_ = 0; + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -159,10 +162,16 @@ hardware_interface::return_type RRBotModularJoint::read( ss << "Reading states:"; // Simulate RRBot's movement - hw_joint_state_ = hw_joint_state_ + (hw_joint_command_ - hw_joint_state_) / hw_slowdown_; + ss << "Reading states:"; - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_joint_state_ << " for joint '" << info_.joints[0].name.c_str() << "'"; + for (const auto & [name, descr] : joint_state_interfaces_) + { + // Simulate RRBot's movement + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << get_state(name) << " for joint '" << name << "'"; + } RCLCPP_INFO(get_logger(), "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -177,10 +186,12 @@ hardware_interface::return_type ros2_control_demo_example_6::RRBotModularJoint:: std::stringstream ss; ss << "Writing commands:"; - // Simulate sending commands to the hardware - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_joint_command_ << " for joint '" << info_.joints[0].name.c_str() << "'"; - + for (const auto & [name, descr] : joint_command_interfaces_) + { + // Simulate sending commands to the hardware + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << get_command(name) << " for joint '" << name << "'"; + } RCLCPP_INFO(get_logger(), "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_7/doc/userdoc.rst b/example_7/doc/userdoc.rst index 50402591..c2a4e6b5 100644 --- a/example_7/doc/userdoc.rst +++ b/example_7/doc/userdoc.rst @@ -193,13 +193,14 @@ In ros2_control, hardware system components are integrated via user defined driv The following code blocks will explain the requirements for writing a new hardware interface. -The hardware plugin for the tutorial robot is a class called ``RobotSystem`` that inherits from ``hardware_interface::SystemInterface``. The ``SystemInterface`` is one of the offered hardware interfaces designed for a complete robot system. For example, The UR5 uses this interface. The ``RobotSystem`` must implement five public methods. +The hardware plugin for the tutorial robot is a class called ``RobotSystem`` that inherits from ``hardware_interface::SystemInterface``. The ``SystemInterface`` is one of the offered hardware interfaces designed for a complete robot system. For example, The UR5 uses this interface. The ``RobotSystem`` must implement the following public methods. 1. ``on_init`` -2. ``export_state_interfaces`` -3. ``export_command_interfaces`` -4. ``read`` -5. ``write`` +2. ``on_configure`` +3. ``read`` +4. ``write`` + +There are more methods that can be implemented for lifecycle changes, but they are not required for the tutorial robot. .. code-block:: c++ @@ -209,15 +210,14 @@ The hardware plugin for the tutorial robot is a class called ``RobotSystem`` tha class HARDWARE_INTERFACE_PUBLIC RobotSystem : public hardware_interface::SystemInterface { public: CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; - std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; // private members // ... } -The ``on_init`` method is called once during ros2_control initialization if the ``RobotSystem`` was specified in the URDF. In this method, communication between the robot hardware needs to be setup and memory dynamic should be allocated. Since the tutorial robot is simulated, explicit communication will not be established. Instead, vectors will be initialized that represent the state all the hardware, e.g. a vector of doubles describing joint angles, etc. +The ``on_init`` method is called once during ros2_control initialization if the ``RobotSystem`` was specified in the URDF. In this method, communication between the robot hardware needs to be setup and memory dynamic should be allocated. Since the tutorial robot is simulated, explicit communication will not be established. .. code-block:: c++ @@ -232,26 +232,15 @@ The ``on_init`` method is called once during ros2_control initialization if the Notably, the behavior of ``on_init`` is expected to vary depending on the URDF file. The ``SystemInterface::on_init(info)`` call fills out the ``info`` object with specifics from the URDF. For example, the ``info`` object has fields for joints, sensors, gpios, and more. Suppose the sensor field has a name value of ``tcp_force_torque_sensor``. Then the ``on_init`` must try to establish communication with that sensor. If it fails, then an error value is returned. -Next, ``export_state_interfaces`` and ``export_command_interfaces`` methods are called in succession. The ``export_state_interfaces`` method returns a vector of ``StateInterface``, describing the ``state_interfaces`` for each joint. The ``StateInterface`` objects are read only data handles. Their constructors require an interface name, interface type, and a pointer to a double data value. For the ``RobotSystem``, the data pointers reference class member variables. This way, the data can be access from every method. - -.. code-block:: c++ - - std::vector RobotSystem::export_state_interfaces() { - std::vector state_interfaces; - // add state interfaces to ``state_interfaces`` for each joint, e.g. `info_.joints[0].state_interfaces_`, `info_.joints[1].state_interfaces_`, `info_.joints[2].state_interfaces_` ... - // ... - return state_interfaces; - } - -The ``export_command_interfaces`` method is nearly identical to the previous one. The difference is that a vector of ``CommandInterface`` is returned. The vector contains objects describing the ``command_interfaces`` for each joint. +The ``on_configure`` method is called once during ros2_control lifecycle of the ``RobotSystem``. Initial values are set for state and command interfaces that represent the state all the hardware, e.g. doubles describing joint angles, etc. .. code-block:: c++ - std::vector RobotSystem::export_command_interfaces() { - std::vector command_interfaces; - // add command interfaces to ``command_interfaces`` for each joint, e.g. `info_.joints[0].command_interfaces_`, `info_.joints[1].command_interfaces_`, `info_.joints[2].command_interfaces_` ... + CallbackReturn RobotSystem::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) + // setup communication with robot hardware // ... - return command_interfaces; + return CallbackReturn::SUCCESS; } The ``read`` method is core method in the ros2_control loop. During the main loop, ros2_control loops over all hardware components and calls the ``read`` method. It is executed on the realtime thread, hence the method must obey by realtime constraints. The ``read`` method is responsible for updating the data values of the ``state_interfaces``. Since the data value point to class member variables, those values can be filled with their corresponding sensor values, which will in turn update the values of each exported ``StateInterface`` object. diff --git a/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp b/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp index 17688a80..69da3d74 100644 --- a/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp +++ b/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp @@ -36,25 +36,13 @@ class RobotSystem : public hardware_interface::SystemInterface public: CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; protected: - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector joint_position_command_; - std::vector joint_velocities_command_; - std::vector joint_position_; - std::vector joint_velocities_; - std::vector ft_states_; - std::vector ft_command_; - - std::unordered_map> joint_interfaces = { - {"position", {}}, {"velocity", {}}}; }; } // namespace ros2_control_demo_example_7 diff --git a/example_7/hardware/r6bot_hardware.cpp b/example_7/hardware/r6bot_hardware.cpp index 7414b8fc..4f710eb6 100644 --- a/example_7/hardware/r6bot_hardware.cpp +++ b/example_7/hardware/r6bot_hardware.cpp @@ -16,6 +16,8 @@ #include #include +#include + namespace ros2_control_demo_example_7 { CallbackReturn RobotSystem::on_init(const hardware_interface::HardwareInfo & info) @@ -24,95 +26,39 @@ CallbackReturn RobotSystem::on_init(const hardware_interface::HardwareInfo & inf { return CallbackReturn::ERROR; } - - // robot has 6 joints and 2 interfaces - joint_position_.assign(6, 0); - joint_velocities_.assign(6, 0); - joint_position_command_.assign(6, 0); - joint_velocities_command_.assign(6, 0); - - // force sensor has 6 readings - ft_states_.assign(6, 0); - ft_command_.assign(6, 0); - - for (const auto & joint : info_.joints) - { - for (const auto & interface : joint.state_interfaces) - { - joint_interfaces[interface.name].push_back(joint.name); - } - } - - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -std::vector RobotSystem::export_state_interfaces() +CallbackReturn RobotSystem::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - std::vector state_interfaces; - - int ind = 0; - for (const auto & joint_name : joint_interfaces["position"]) + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) { - state_interfaces.emplace_back(joint_name, "position", &joint_position_[ind++]); + set_state(name, 0.0); } - - ind = 0; - for (const auto & joint_name : joint_interfaces["velocity"]) + for (const auto & [name, descr] : joint_command_interfaces_) { - state_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_[ind++]); + set_command(name, 0.0); } - - state_interfaces.emplace_back("tcp_fts_sensor", "force.x", &ft_states_[0]); - state_interfaces.emplace_back("tcp_fts_sensor", "force.y", &ft_states_[1]); - state_interfaces.emplace_back("tcp_fts_sensor", "force.z", &ft_states_[2]); - state_interfaces.emplace_back("tcp_fts_sensor", "torque.x", &ft_states_[3]); - state_interfaces.emplace_back("tcp_fts_sensor", "torque.y", &ft_states_[4]); - state_interfaces.emplace_back("tcp_fts_sensor", "torque.z", &ft_states_[5]); - - return state_interfaces; -} - -std::vector RobotSystem::export_command_interfaces() -{ - std::vector command_interfaces; - - int ind = 0; - for (const auto & joint_name : joint_interfaces["position"]) - { - command_interfaces.emplace_back(joint_name, "position", &joint_position_command_[ind++]); - } - - ind = 0; - for (const auto & joint_name : joint_interfaces["velocity"]) + for (const auto & [name, descr] : sensor_state_interfaces_) { - command_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_command_[ind++]); + set_state(name, 0.0); } - command_interfaces.emplace_back("tcp_fts_sensor", "force.x", &ft_command_[0]); - command_interfaces.emplace_back("tcp_fts_sensor", "force.y", &ft_command_[1]); - command_interfaces.emplace_back("tcp_fts_sensor", "force.z", &ft_command_[2]); - command_interfaces.emplace_back("tcp_fts_sensor", "torque.x", &ft_command_[3]); - command_interfaces.emplace_back("tcp_fts_sensor", "torque.y", &ft_command_[4]); - command_interfaces.emplace_back("tcp_fts_sensor", "torque.z", &ft_command_[5]); - - return command_interfaces; + return CallbackReturn::SUCCESS; } return_type RobotSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // TODO(pac48) set sensor_states_ values from subscriber - for (auto i = 0ul; i < joint_velocities_command_.size(); i++) - { - joint_velocities_[i] = joint_velocities_command_[i]; - joint_position_[i] += joint_velocities_command_[i] * period.seconds(); - } - - for (auto i = 0ul; i < joint_position_command_.size(); i++) + for (std::size_t i = 0; i < info_.joints.size(); i++) { - joint_position_[i] = joint_position_command_[i]; + const auto name_vel = info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY; + const auto name_pos = info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION; + set_state(name_vel, get_command(name_vel)); + set_state(name_pos, get_state(name_pos) + get_state(name_vel) * period.seconds()); } - return return_type::OK; } diff --git a/example_7/reference_generator/send_trajectory.cpp b/example_7/reference_generator/send_trajectory.cpp index e6d3fd78..3de586d1 100644 --- a/example_7/reference_generator/send_trajectory.cpp +++ b/example_7/reference_generator/send_trajectory.cpp @@ -96,6 +96,15 @@ int main(int argc, char ** argv) trajectory_msg.points.push_back(trajectory_point_msg); } + // send zero velocities in the end + std::fill(trajectory_point_msg.velocities.begin(), trajectory_point_msg.velocities.end(), 0.0); + trajectory_point_msg.time_from_start.sec = trajectory_len / loop_rate; + trajectory_point_msg.time_from_start.nanosec = static_cast( + 1E9 / loop_rate * + static_cast( + trajectory_len - loop_rate * (trajectory_len / loop_rate))); // implicit integer division + trajectory_msg.points.push_back(trajectory_point_msg); + pub->publish(trajectory_msg); while (rclcpp::ok()) { diff --git a/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp b/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp index fc01cb2b..a5a8d2d8 100644 --- a/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp +++ b/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp @@ -41,10 +41,6 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; diff --git a/example_8/hardware/rrbot_transmissions_system_position_only.cpp b/example_8/hardware/rrbot_transmissions_system_position_only.cpp index 594ec13f..37ca0084 100644 --- a/example_8/hardware/rrbot_transmissions_system_position_only.cpp +++ b/example_8/hardware/rrbot_transmissions_system_position_only.cpp @@ -164,49 +164,32 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: reset_interfaces(joint_interfaces_); reset_interfaces(actuator_interfaces_); - RCLCPP_INFO(get_logger(), "Configuration successful"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotTransmissionsSystemPositionOnlyHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (const auto & joint : info_.joints) + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) { - /// @pre all joint interfaces exist, checked in on_init() - auto joint_interface = std::find_if( - joint_interfaces_.begin(), joint_interfaces_.end(), - [&](const InterfaceData & interface) { return interface.name_ == joint.name; }); - - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint.name, hardware_interface::HW_IF_POSITION, &joint_interface->state_)); + set_state(name, 0.0); } - return state_interfaces; -} - -std::vector -RRBotTransmissionsSystemPositionOnlyHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (const auto & joint : info_.joints) + for (const auto & [name, descr] : joint_command_interfaces_) { - /// @pre all joint interfaces exist, checked in on_init() - auto joint_interface = std::find_if( - joint_interfaces_.begin(), joint_interfaces_.end(), - [&](const InterfaceData & interface) { return interface.name_ == joint.name; }); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joint.name, hardware_interface::HW_IF_POSITION, &joint_interface->command_)); + set_command(name, 0.0); } - return command_interfaces; + + RCLCPP_INFO(get_logger(), "Configuration successful"); + + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(get_logger(), "Activating..."); + + // command and state should be equal when starting + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_command(name, get_state(name)); + } + RCLCPP_INFO(get_logger(), "Activation successful"); return hardware_interface::CallbackReturn::SUCCESS; @@ -263,12 +246,30 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); + // update internal storage from resource_manager + std::for_each( + joint_interfaces_.begin(), joint_interfaces_.end(), + [this](auto & joint_interface) + { + set_state( + joint_interface.name_ + "/" + hardware_interface::HW_IF_POSITION, joint_interface.state_); + }); + return hardware_interface::return_type::OK; } hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + // update internal storage from resource_manager + std::for_each( + joint_interfaces_.begin(), joint_interfaces_.end(), + [this](auto & joint_interface) + { + joint_interface.command_ = + get_command(joint_interface.name_ + "/" + hardware_interface::HW_IF_POSITION); + }); + // joint: command -> transmission std::for_each( joint_interfaces_.begin(), joint_interfaces_.end(), [](auto & joint_interface) diff --git a/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp b/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp index f9b3ea82..7643e7a8 100644 --- a/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp +++ b/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp @@ -40,10 +40,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -61,10 +57,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; - - // Store the command for the simulated robot - std::vector hw_commands_; - std::vector hw_states_; }; } // namespace ros2_control_demo_example_9 diff --git a/example_9/hardware/rrbot.cpp b/example_9/hardware/rrbot.cpp index ba1dc6ce..7e136dc9 100644 --- a/example_9/hardware/rrbot.cpp +++ b/example_9/hardware/rrbot.cpp @@ -43,8 +43,6 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); for (const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -100,41 +98,17 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { - hw_states_[i] = 0; - hw_commands_[i] = 0; + set_state(name, 0.0); } - - RCLCPP_INFO(get_logger(), "Successfully configured!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSystemPositionOnlyHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); - } - - return state_interfaces; -} - -std::vector -RRBotSystemPositionOnlyHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + set_command(name, 0.0); } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( @@ -151,9 +125,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { - hw_commands_[i] = hw_states_[i]; + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -186,13 +160,13 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( std::stringstream ss; ss << "Reading states:"; - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; - + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name.c_str() << "'"; + << "\t" << get_state(name) << " for joint '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -207,11 +181,11 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( std::stringstream ss; ss << "Writing commands:"; - for (uint i = 0; i < hw_commands_.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { // Simulate sending commands to the hardware ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name.c_str() << "'"; + << "\t" << get_command(name) << " for joint '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code