diff --git a/.github/dependabot.yml b/.github/dependabot.yml
index aafd67c236..f5e9921f23 100644
--- a/.github/dependabot.yml
+++ b/.github/dependabot.yml
@@ -18,10 +18,3 @@ updates:
schedule:
interval: "weekly"
target-branch: "humble"
- - package-ecosystem: "github-actions"
- # Workflow files stored in the
- # default location of `.github/workflows`
- directory: "/"
- schedule:
- interval: "weekly"
- target-branch: "iron"
diff --git a/.github/mergify.yml b/.github/mergify.yml
index 39ee6b6bc0..fd185e02d0 100644
--- a/.github/mergify.yml
+++ b/.github/mergify.yml
@@ -32,7 +32,7 @@ pull_request_rules:
- author=mergify[bot]
actions:
comment:
- message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich?
+ message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor?
- name: development targets master branch
conditions:
@@ -40,6 +40,7 @@ pull_request_rules:
- author!=bmagyar
- author!=destogl
- author!=christophfroehlich
+ - author!=saikishor
- author!=mergify[bot]
- author!=dependabot[bot]
actions:
diff --git a/.github/workflows/README.md b/.github/workflows/README.md
index 62007ffc2d..2ad6a249fe 100644
--- a/.github/workflows/README.md
+++ b/.github/workflows/README.md
@@ -3,5 +3,4 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages
:---------: | :----: | :----------: | :-----------: | :---------------:
**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-debian-build.yml)
[](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-rhel-binary-build.yml) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling)
**Jazzy** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-source-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-debian-build.yml)
[](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-rhel-binary-build.yml) | [Documentation](https://control.ros.org/jazzy/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#jazzy)
-**Iron** | [`iron`](https://github.com/ros-controls/ros2_control/tree/master) | [](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/iron-source-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/iron-debian-build.yml)
[](https://github.com/ros-controls/ros2_control/actions/workflows/iron-rhel-binary-build.yml) | [Documentation](https://control.ros.org/iron/index.html)
[API Reference](https://control.ros.org/iron/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#iron)
**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/humble-debian-build.yml)
[](https://github.com/ros-controls/ros2_control/actions/workflows/humble-rhel-binary-build.yml) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble)
diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml
index 560ac37cff..96938467f9 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-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml
deleted file mode 100644
index c2d9c19110..0000000000
--- a/.github/workflows/iron-abi-compatibility.yml
+++ /dev/null
@@ -1,27 +0,0 @@
-name: Iron - ABI Compatibility Check
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-abi-compatibility.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control-not-released.iron.repos'
-
-jobs:
- abi_check:
- runs-on: ubuntu-latest
- steps:
- - uses: actions/checkout@v4
- - uses: ros-industrial/industrial_ci@master
- env:
- ROS_DISTRO: iron
- ROS_REPO: testing
- ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }}
- NOT_TEST_BUILD: true
diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml
deleted file mode 100644
index ef90e256a0..0000000000
--- a/.github/workflows/iron-binary-build.yml
+++ /dev/null
@@ -1,47 +0,0 @@
-name: Iron Binary Build
-# author: Denis Štogl
-# description: 'Build & test all dependencies from released (binary) packages.'
-
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-binary-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control-not-released.iron.repos'
- push:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-binary-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control-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-not-released.iron.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 e9295dad44..0000000000
--- a/.github/workflows/iron-check-docs.yml
+++ /dev/null
@@ -1,18 +0,0 @@
-name: Iron Check Docs
-
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.rst'
- - '**.md'
- - '**.yaml'
-
-jobs:
- check-docs:
- name: Check Docs
- uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@iron
- with:
- ROS2_CONTROL_PR: ${{ github.ref }}
diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml
deleted file mode 100644
index ff5be81d7d..0000000000
--- a/.github/workflows/iron-coverage-build.yml
+++ /dev/null
@@ -1,36 +0,0 @@
-name: Coverage Build - Iron
-on:
- workflow_dispatch:
- push:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-coverage-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- - 'codecov.yml'
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-coverage-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- - 'codecov.yml'
-
-jobs:
- coverage_iron:
- uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master
- secrets: inherit
- with:
- ros_distro: iron
diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml
deleted file mode 100644
index 3cbe0c5127..0000000000
--- a/.github/workflows/iron-debian-build.yml
+++ /dev/null
@@ -1,33 +0,0 @@
-name: Debian Iron Source Build
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-debian-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- schedule:
- # Run every day to detect flakiness and broken dependencies
- - cron: '03 1 * * *'
-
-
-jobs:
- debian_source_build:
- uses: ros-controls/ros2_control_ci/.github/workflows/reusable-debian-build.yml@master
- strategy:
- fail-fast: false
- matrix:
- ROS_DISTRO: [iron]
- with:
- ros_distro: ${{ matrix.ROS_DISTRO }}
- upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos
- ref_for_scheduled_build: master
- skip_packages: rqt_controller_manager
- skip_packages_test: controller_manager_msgs
diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml
deleted file mode 100644
index a128958031..0000000000
--- 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-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml
deleted file mode 100644
index f308c495f3..0000000000
--- a/.github/workflows/iron-rhel-binary-build.yml
+++ /dev/null
@@ -1,31 +0,0 @@
-name: RHEL Iron Semi-Binary Build
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-rhel-binary-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- schedule:
- # Run every day to detect flakiness and broken dependencies
- - cron: '03 1 * * *'
-
-jobs:
- rhel_semi_binary_build:
- uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rhel-binary-build.yml@master
- strategy:
- fail-fast: false
- matrix:
- ROS_DISTRO: [iron]
- with:
- ros_distro: ${{ matrix.ROS_DISTRO }}
- upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos
- ref_for_scheduled_build: iron
- skip_packages: rqt_controller_manager
diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml
deleted file mode 100644
index 30a83e5367..0000000000
--- a/.github/workflows/iron-semi-binary-build.yml
+++ /dev/null
@@ -1,47 +0,0 @@
-name: Iron Semi-Binary Build
-# author: Denis Štogl
-# description: 'Build & test all dependencies from released (binary) packages.'
-
-on:
- workflow_dispatch:
- pull_request:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-semi-binary-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- push:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-semi-binary-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.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.iron.repos
- ref_for_scheduled_build: iron
diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml
deleted file mode 100644
index 3b7c53f6ff..0000000000
--- a/.github/workflows/iron-source-build.yml
+++ /dev/null
@@ -1,27 +0,0 @@
-name: Iron Source Build
-on:
- workflow_dispatch:
- push:
- branches:
- - iron
- paths:
- - '**.hpp'
- - '**.h'
- - '**.cpp'
- - '**.py'
- - '.github/workflows/iron-source-build.yml'
- - '**/package.xml'
- - '**/CMakeLists.txt'
- - 'ros2_control.iron.repos'
- schedule:
- # Run every day to detect flakiness and broken dependencies
- - cron: '03 3 * * *'
-
-jobs:
- source:
- uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master
- with:
- ros_distro: iron
- ref: iron
- ros2_repo_branch: iron
- os_name: ubuntu-22.04
diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml
index 9634732cf9..12769eb740 100644
--- a/.github/workflows/jazzy-semi-binary-build.yml
+++ b/.github/workflows/jazzy-semi-binary-build.yml
@@ -39,7 +39,7 @@ jobs:
fail-fast: false
matrix:
ROS_DISTRO: [jazzy]
- ROS_REPO: [main, testing]
+ ROS_REPO: [testing]
with:
ros_distro: ${{ matrix.ROS_DISTRO }}
ros_repo: ${{ matrix.ROS_REPO }}
diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml
new file mode 100644
index 0000000000..b55091521e
--- /dev/null
+++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml
@@ -0,0 +1,44 @@
+name: Check Rolling Compatibility on Humble
+# author: Christoph Froehlich
+# description: 'Build & test the rolling version on Humble distro.'
+
+on:
+ workflow_dispatch:
+ pull_request:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-compatibility-humble-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.rolling.repos'
+ push:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-compatibility-humble-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.rolling.repos'
+
+jobs:
+ build-on-humble:
+ uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+ strategy:
+ fail-fast: false
+ matrix:
+ ROS_DISTRO: [humble]
+ ROS_REPO: [testing]
+ with:
+ ros_distro: ${{ matrix.ROS_DISTRO }}
+ ros_repo: ${{ matrix.ROS_REPO }}
+ upstream_workspace: ros2_control.rolling.repos
+ ref_for_scheduled_build: master
diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml
new file mode 100644
index 0000000000..621ffb6a26
--- /dev/null
+++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml
@@ -0,0 +1,44 @@
+name: Check Rolling Compatibility on Jazzy
+# author: Christoph Froehlich
+# description: 'Build & test the rolling version on Jazzy distro.'
+
+on:
+ workflow_dispatch:
+ pull_request:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.rolling.repos'
+ push:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.rolling.repos'
+
+jobs:
+ build-on-jazzy:
+ uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+ strategy:
+ fail-fast: false
+ matrix:
+ ROS_DISTRO: [jazzy]
+ ROS_REPO: [testing]
+ with:
+ ros_distro: ${{ matrix.ROS_DISTRO }}
+ ros_repo: ${{ matrix.ROS_REPO }}
+ upstream_workspace: ros2_control.rolling.repos
+ ref_for_scheduled_build: master
diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml
index 4cdb7ab585..492da45ab9 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]
- ROS_REPO: [main, testing]
+ ROS_REPO: [testing]
with:
ros_distro: ${{ matrix.ROS_DISTRO }}
ros_repo: ${{ matrix.ROS_REPO }}
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 63e7f08682..205e0f63ab 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -16,7 +16,7 @@
repos:
# Standard hooks
- repo: https://github.com/pre-commit/pre-commit-hooks
- rev: v4.6.0
+ rev: v5.0.0
hooks:
- id: check-added-large-files
- id: check-ast
@@ -37,7 +37,7 @@ repos:
# Python hooks
- repo: https://github.com/asottile/pyupgrade
- rev: v3.17.0
+ rev: v3.19.0
hooks:
- id: pyupgrade
args: [--py36-plus]
@@ -50,7 +50,7 @@ repos:
args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
- repo: https://github.com/psf/black
- rev: 24.8.0
+ rev: 24.10.0
hooks:
- id: black
args: ["--line-length=99"]
@@ -63,7 +63,7 @@ repos:
# CPP hooks
- repo: https://github.com/pre-commit/mirrors-clang-format
- rev: v19.1.0
+ rev: v19.1.3
hooks:
- id: clang-format
args: ['-fallback-style=none', '-i']
@@ -133,7 +133,7 @@ repos:
exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
- repo: https://github.com/python-jsonschema/check-jsonschema
- rev: 0.29.3
+ rev: 0.29.4
hooks:
- id: check-github-workflows
args: ["--verbose"]
diff --git a/README.md b/README.md
index 40d2a3c189..47c6ad1523 100644
--- a/README.md
+++ b/README.md
@@ -13,7 +13,6 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages
:---------: | :----: | :----------: | :-----------: | :---------------:
**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/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) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling)
**Jazzy** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/jazzy-semi-binary-build.yml?branch=master) | [Documentation](https://control.ros.org/jazzy/index.html)
[API Reference](https://control.ros.org/jazzy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#jazzy)
-**Iron** | [`iron`](https://github.com/ros-controls/ros2_control/tree/iron) | [](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build.yml?branch=iron)
[](https://github.com/ros-controls/ros2_control/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) | [ros2_control](https://index.ros.org/p/ros2_control/#iron)
**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build.yml?branch=master)
[](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build.yml?branch=master) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble)
[Detailed build status](.github/workflows/README.md)
diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst
index 173f6f1bcf..665b0175f6 100644
--- a/controller_interface/CHANGELOG.rst
+++ b/controller_interface/CHANGELOG.rst
@@ -2,6 +2,21 @@
Changelog for package controller_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.20.0 (2024-11-08)
+-------------------
+* reset the async variables upon activation to work post exceptions (`#1860 `_)
+* [CM] Fix controller missing update cycles in a real setup (`#1774 `_)
+* Contributors: Sai Kishor Kothakota
+
+4.19.0 (2024-10-26)
+-------------------
+* [CM] Async Function Handler for Controllers (`#1489 `_)
+* Check the update_rate set to the controllers to be a valid one (`#1788 `_)
+* [PR-1689] Follow-up PR of the controller interface variants integration (`#1779 `_)
+* Add `PoseSensor` semantic component (`#1775 `_)
+* [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_)
+* Contributors: RobertWilbrandt, Sai Kishor Kothakota
+
4.18.0 (2024-10-07)
-------------------
* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_)
diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt
index cad5810ee5..85294c68d1 100644
--- a/controller_interface/CMakeLists.txt
+++ b/controller_interface/CMakeLists.txt
@@ -9,6 +9,7 @@ endif()
set(THIS_PACKAGE_INCLUDE_DEPENDS
hardware_interface
rclcpp_lifecycle
+ realtime_tools
)
find_package(ament_cmake REQUIRED)
diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp
deleted file mode 100644
index 357b3a2ce3..0000000000
--- a/controller_interface/include/controller_interface/async_controller.hpp
+++ /dev/null
@@ -1,112 +0,0 @@
-// Copyright 2024 ros2_control development team
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_
-#define CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_
-
-#include
-#include
-#include
-
-#include "controller_interface_base.hpp"
-#include "lifecycle_msgs/msg/state.hpp"
-
-namespace controller_interface
-{
-
-class AsyncControllerThread
-{
-public:
- /// Constructor for the AsyncControllerThread object.
- /**
- *
- * \param[in] controller shared pointer to a controller.
- * \param[in] cm_update_rate the controller manager's update rate.
- */
- AsyncControllerThread(
- std::shared_ptr & controller, int cm_update_rate)
- : terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate)
- {
- }
-
- AsyncControllerThread(const AsyncControllerThread & t) = delete;
- AsyncControllerThread(AsyncControllerThread && t) = delete;
-
- // Destructor, called when the component is erased from its map.
- ~AsyncControllerThread()
- {
- terminated_.store(true, std::memory_order_seq_cst);
- if (thread_.joinable())
- {
- thread_.join();
- }
- }
-
- /// Creates the controller's thread.
- /**
- * Called when the controller is activated.
- *
- */
- void activate()
- {
- thread_ = std::thread(&AsyncControllerThread::controller_update_callback, this);
- }
-
- /// Periodically execute the controller's update method.
- /**
- * Callback of the async controller's thread.
- * **Not synchronized with the controller manager's write and read currently**
- *
- */
- void controller_update_callback()
- {
- using TimePoint = std::chrono::system_clock::time_point;
- unsigned int used_update_rate =
- controller_->get_update_rate() == 0 ? cm_update_rate_ : controller_->get_update_rate();
-
- auto previous_time = controller_->get_node()->now();
- while (!terminated_.load(std::memory_order_relaxed))
- {
- auto const period = std::chrono::nanoseconds(1'000'000'000 / used_update_rate);
- TimePoint next_iteration_time =
- TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds()));
-
- if (
- controller_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
- {
- auto const current_time = controller_->get_node()->now();
- auto const measured_period = current_time - previous_time;
- previous_time = current_time;
- controller_->update(
- controller_->get_node()->now(),
- (controller_->get_update_rate() != cm_update_rate_ && controller_->get_update_rate() != 0)
- ? rclcpp::Duration::from_seconds(1.0 / controller_->get_update_rate())
- : measured_period);
- }
-
- next_iteration_time += period;
- std::this_thread::sleep_until(next_iteration_time);
- }
- }
-
-private:
- std::atomic terminated_;
- std::shared_ptr controller_;
- std::thread thread_;
- unsigned int cm_update_rate_;
-};
-
-} // namespace controller_interface
-
-#endif // CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_
diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp
index 2bd01cc326..ae3804919c 100644
--- a/controller_interface/include/controller_interface/controller_interface_base.hpp
+++ b/controller_interface/include/controller_interface/controller_interface_base.hpp
@@ -17,9 +17,11 @@
#include
#include
+#include
#include
#include "controller_interface/visibility_control.h"
+#include "realtime_tools/async_function_handler.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
@@ -58,6 +60,17 @@ struct InterfaceConfiguration
std::vector names = {};
};
+struct ControllerUpdateStats
+{
+ void reset()
+ {
+ total_triggers = 0;
+ failed_triggers = 0;
+ }
+
+ unsigned int total_triggers;
+ unsigned int failed_triggers;
+};
/**
* Base interface class for an controller. The interface may not be used to implement a controller.
* The class provides definitions for `ControllerInterface` and `ChainableControllerInterface`
@@ -147,12 +160,29 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
* **The method called in the (real-time) control loop.**
*
* \param[in] time The time at the start of this control loop iteration
- * \param[in] period The measured time taken by the last control loop iteration
+ * \param[in] period The measured time since the last control loop iteration
* \returns return_type::OK if update is successfully, otherwise return_type::ERROR.
*/
CONTROLLER_INTERFACE_PUBLIC
virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
+ /**
+ * Trigger update method. This method is used by the controller_manager to trigger the update
+ * method of the controller.
+ * The method is used to trigger the update method of the controller synchronously or
+ * asynchronously, based on the controller configuration.
+ * **The method called in the (real-time) control loop.**
+ *
+ * \param[in] time The time at the start of this control loop iteration
+ * \param[in] period The measured time taken by the last control loop iteration
+ * \returns A pair with the first element being a boolean indicating if the async callback method
+ * was triggered and the second element being the last return value of the async function. For
+ * more details check the AsyncFunctionHandler implementation in `realtime_tools` package.
+ */
+ CONTROLLER_INTERFACE_PUBLIC
+ std::pair trigger_update(
+ const rclcpp::Time & time, const rclcpp::Duration & period);
+
CONTROLLER_INTERFACE_PUBLIC
std::shared_ptr get_node();
@@ -270,15 +300,30 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
CONTROLLER_INTERFACE_PUBLIC
virtual bool is_in_chained_mode() const = 0;
+ /**
+ * Method to wait for any running async update cycle to finish after finishing the current cycle.
+ * This is needed to be called before deactivating the controller by the controller_manager, so
+ * that the interfaces still exist when the controller finishes its cycle and then it's exits.
+ *
+ * \note **The method is not real-time safe and shouldn't be called in the control loop.**
+ *
+ * If the controller is running in async mode, the method will wait for the current async update
+ * to finish. If the controller is not running in async mode, the method will do nothing.
+ */
+ CONTROLLER_INTERFACE_PUBLIC
+ void wait_for_trigger_update_to_finish();
+
protected:
std::vector command_interfaces_;
std::vector state_interfaces_;
private:
std::shared_ptr node_;
+ std::unique_ptr> async_handler_;
unsigned int update_rate_ = 0;
bool is_async_ = false;
std::string urdf_ = "";
+ ControllerUpdateStats trigger_stats_;
};
using ControllerInterfaceBaseSharedPtr = std::shared_ptr;
diff --git a/controller_interface/package.xml b/controller_interface/package.xml
index dce1d79f86..ca3fc2af49 100644
--- a/controller_interface/package.xml
+++ b/controller_interface/package.xml
@@ -2,7 +2,7 @@
controller_interface
- 4.18.0
+ 4.20.0
Description of controller_interface
Bence Magyar
Denis Štogl
@@ -12,12 +12,16 @@
ament_cmake_gen_version_h
hardware_interface
+ realtime_tools
rclcpp_lifecycle
sensor_msgs
hardware_interface
+ realtime_tools
rclcpp_lifecycle
+ realtime_tools
+
ament_cmake_gmock
geometry_msgs
sensor_msgs
diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
index e2c1fa480a..0713ec3c25 100644
--- a/controller_interface/src/controller_interface_base.cpp
+++ b/controller_interface/src/controller_interface_base.cpp
@@ -16,7 +16,6 @@
#include
#include
-#include
#include
#include "lifecycle_msgs/msg/state.hpp"
@@ -37,6 +36,7 @@ return_type ControllerInterfaceBase::init(
{
auto_declare("update_rate", update_rate_);
auto_declare("is_async", false);
+ auto_declare("thread_priority", 50);
}
catch (const std::exception & e)
{
@@ -57,10 +57,25 @@ return_type ControllerInterfaceBase::init(
std::bind(&ControllerInterfaceBase::on_configure, this, std::placeholders::_1));
node_->register_on_cleanup(
- std::bind(&ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1));
+ [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+ {
+ if (is_async() && async_handler_ && async_handler_->is_running())
+ {
+ async_handler_->stop_thread();
+ }
+ return on_cleanup(previous_state);
+ });
node_->register_on_activate(
- std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1));
+ [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+ {
+ if (is_async() && async_handler_ && async_handler_->is_running())
+ {
+ // This is needed if it is disabled due to a thrown exception in the async callback thread
+ async_handler_->reset_variables();
+ }
+ return on_activate(previous_state);
+ });
node_->register_on_deactivate(
std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1));
@@ -106,6 +121,21 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
}
is_async_ = get_node()->get_parameter("is_async").as_bool();
}
+ if (is_async_)
+ {
+ const unsigned int thread_priority =
+ static_cast(get_node()->get_parameter("thread_priority").as_int());
+ RCLCPP_INFO(
+ get_node()->get_logger(), "Starting async handler with scheduler priority: %d",
+ thread_priority);
+ async_handler_ = std::make_unique>();
+ async_handler_->init(
+ std::bind(
+ &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2),
+ thread_priority);
+ async_handler_->start_thread();
+ }
+ trigger_stats_.reset();
return get_node()->configure();
}
@@ -129,6 +159,29 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c
return node_->get_current_state();
}
+std::pair ControllerInterfaceBase::trigger_update(
+ const rclcpp::Time & time, const rclcpp::Duration & period)
+{
+ trigger_stats_.total_triggers++;
+ if (is_async())
+ {
+ const auto result = async_handler_->trigger_async_callback(time, period);
+ if (!result.first)
+ {
+ trigger_stats_.failed_triggers++;
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(), *get_node()->get_clock(), 20000,
+ "The controller missed %u update cycles out of %u total triggers.",
+ trigger_stats_.failed_triggers, trigger_stats_.total_triggers);
+ }
+ return result;
+ }
+ else
+ {
+ return std::make_pair(true, update(time, period));
+ }
+}
+
std::shared_ptr ControllerInterfaceBase::get_node()
{
if (!node_.get())
@@ -153,4 +206,11 @@ bool ControllerInterfaceBase::is_async() const { return is_async_; }
const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; }
+void ControllerInterfaceBase::wait_for_trigger_update_to_finish()
+{
+ if (is_async() && async_handler_ && async_handler_->is_running())
+ {
+ async_handler_->wait_for_trigger_cycle_to_finish();
+ }
+}
} // namespace controller_interface
diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst
index f714f056d9..376b15014d 100644
--- a/controller_manager/CHANGELOG.rst
+++ b/controller_manager/CHANGELOG.rst
@@ -2,6 +2,34 @@
Changelog for package controller_manager
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.20.0 (2024-11-08)
+-------------------
+* change from thread_priority.hpp to realtime_helpers.hpp (`#1829 `_)
+* Use Clock instead of Rate for backward compatibility of rolling (`#1864 `_)
+* [ros2_control_node] Handle simulation environment clocks (`#1810 `_)
+* [CM] Fix controller missing update cycles in a real setup (`#1774 `_)
+* [ros2_control_node] Add option to set the CPU affinity (`#1852 `_)
+* [ros2_control_node] Add the realtime_tools lock_memory method to prevent page faults (`#1822 `_)
+* Fix CMP0115 (`#1830 `_)
+* fix: typo use thread_priority (`#1844 `_)
+* Fix Hardware spawner and add tests for it (`#1759 `_)
+* add thread_priority option to the ros2_control_node (`#1820 `_)
+* Contributors: Baris Yazici, Christoph Fröhlich, Felix Exner (fexner), Sai Kishor Kothakota
+
+4.19.0 (2024-10-26)
+-------------------
+* Fix timeout value in std output (`#1807 `_)
+* [CM] Async Function Handler for Controllers (`#1489 `_)
+* [Spawner] Add support for wildcard entries in the controller param files (`#1724 `_)
+* [Feature] Fallback controllers (`#1789 `_)
+* Check the update_rate set to the controllers to be a valid one (`#1788 `_)
+* [PR-1689] Follow-up PR of the controller interface variants integration (`#1779 `_)
+* Improve diagnostics of Controllers, Hardware Components and Controller Manager (`#1764 `_)
+* Improve launch utils to support the multiple controller names (`#1782 `_)
+* [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_)
+* [CM] Throw an exception when the components initially fail to be in the required state (`#1729 `_)
+* Contributors: Felix Exner (fexner), Sai Kishor Kothakota
+
4.18.0 (2024-10-07)
-------------------
* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_)
diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
index 3defaead53..9544a7ceab 100644
--- a/controller_manager/CMakeLists.txt
+++ b/controller_manager/CMakeLists.txt
@@ -87,6 +87,7 @@ if(BUILD_TESTING)
ament_add_gmock(test_controller_manager
test/test_controller_manager.cpp
+ TIMEOUT 180
)
target_link_libraries(test_controller_manager
controller_manager
@@ -195,6 +196,15 @@ if(BUILD_TESTING)
ros2_control_test_assets::ros2_control_test_assets
)
+ ament_add_gmock(test_hardware_spawner
+ test/test_hardware_spawner.cpp
+ TIMEOUT 120
+ )
+ target_link_libraries(test_hardware_spawner
+ controller_manager
+ ros2_control_test_assets::ros2_control_test_assets
+ )
+
install(FILES test/test_controller_spawner_with_fallback_controllers.yaml
DESTINATION test)
diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py
index a1d3c0f5ad..909e681ce6 100644
--- a/controller_manager/controller_manager/controller_manager_services.py
+++ b/controller_manager/controller_manager/controller_manager_services.py
@@ -114,7 +114,7 @@ def service_caller(
if future.result() is None:
node.get_logger().warning(
f"Failed getting a result from calling {fully_qualified_service_name} in "
- f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)"
+ f"{call_timeout}. (Attempt {attempt+1} of {max_attempts}.)"
)
else:
return future.result()
@@ -253,24 +253,57 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti
)
-def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name):
+def get_parameter_from_param_file(
+ node, controller_name, namespace, parameter_file, parameter_name
+):
with open(parameter_file) as f:
namespaced_controller = (
- controller_name if namespace == "/" else f"{namespace}/{controller_name}"
+ f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
)
+ WILDCARD_KEY = "/**"
+ ROS_PARAMS_KEY = "ros__parameters"
parameters = yaml.safe_load(f)
- if namespaced_controller in parameters:
- value = parameters[namespaced_controller]
- if not isinstance(value, dict) or "ros__parameters" not in value:
+ controller_param_dict = None
+ # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name'
+ for key in [
+ controller_name,
+ namespaced_controller,
+ f"{WILDCARD_KEY}/{controller_name}",
+ f"{WILDCARD_KEY}{namespaced_controller}",
+ ]:
+ if key in parameters:
+ if key == controller_name and namespace != "/":
+ node.get_logger().fatal(
+ f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}"
+ )
+ break
+ controller_param_dict = parameters[key]
+
+ if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
+ controller_param_dict = parameters[WILDCARD_KEY][key]
+
+ if controller_param_dict and (
+ not isinstance(controller_param_dict, dict)
+ or ROS_PARAMS_KEY not in controller_param_dict
+ ):
raise RuntimeError(
- f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}"
+ f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}"
)
- if parameter_name in parameters[namespaced_controller]["ros__parameters"]:
- return parameters[namespaced_controller]["ros__parameters"][parameter_name]
- else:
- return None
- else:
- return None
+ if (
+ controller_param_dict
+ and ROS_PARAMS_KEY in controller_param_dict
+ and parameter_name in controller_param_dict[ROS_PARAMS_KEY]
+ ):
+ break
+
+ if controller_param_dict is None:
+ node.get_logger().fatal(
+ f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}"
+ )
+ if parameter_name in controller_param_dict[ROS_PARAMS_KEY]:
+ return controller_param_dict[ROS_PARAMS_KEY][parameter_name]
+
+ return None
def set_controller_parameters(
@@ -324,7 +357,7 @@ def set_controller_parameters_from_param_file(
)
controller_type = get_parameter_from_param_file(
- controller_name, spawner_namespace, parameter_file, "type"
+ node, controller_name, spawner_namespace, parameter_file, "type"
)
if controller_type:
if not set_controller_parameters(
@@ -333,7 +366,7 @@ def set_controller_parameters_from_param_file(
return False
fallback_controllers = get_parameter_from_param_file(
- controller_name, spawner_namespace, parameter_file, "fallback_controllers"
+ node, controller_name, spawner_namespace, parameter_file, "fallback_controllers"
)
if fallback_controllers:
if not set_controller_parameters(
diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py
index 29c0b5e97c..4f7afe714c 100644
--- a/controller_manager/controller_manager/hardware_spawner.py
+++ b/controller_manager/controller_manager/hardware_spawner.py
@@ -57,7 +57,7 @@ def has_service_names(node, node_name, node_namespace, service_names):
def is_hardware_component_loaded(
node, controller_manager, hardware_component, service_timeout=0.0
):
- components = list_hardware_components(node, hardware_component, service_timeout).component
+ components = list_hardware_components(node, controller_manager, service_timeout).component
return any(c.name == hardware_component for c in components)
@@ -67,49 +67,45 @@ def handle_set_component_state_service_call(
response = set_hardware_component_state(node, controller_manager_name, component, target_state)
if response.ok and response.state == target_state:
node.get_logger().info(
- bcolors.OKGREEN
- + f"{action} component '{component}'. Hardware now in state: {response.state}."
+ f"{bcolors.OKGREEN}{action} component '{component}'. Hardware now in state: {response.state}.{bcolors.ENDC}"
)
elif response.ok and not response.state == target_state:
node.get_logger().warn(
- bcolors.WARNING
- + f"Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'."
+ f"{bcolors.WARNING}Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'.{bcolors.ENDC}"
)
else:
node.get_logger().warn(
- bcolors.WARNING
- + f"Could not {action} component '{component}'. Service call failed. Wrong component name?"
+ f"{bcolors.WARNING}Could not {action} component '{component}'. Service call failed. Wrong component name?{bcolors.ENDC}"
)
-def activate_components(node, controller_manager_name, components_to_activate):
+def activate_component(node, controller_manager_name, component_to_activate):
active_state = State()
active_state.id = State.PRIMARY_STATE_ACTIVE
active_state.label = "active"
- for component in components_to_activate:
- handle_set_component_state_service_call(
- node, controller_manager_name, component, active_state, "activated"
- )
+ handle_set_component_state_service_call(
+ node, controller_manager_name, component_to_activate, active_state, "activated"
+ )
-def configure_components(node, controller_manager_name, components_to_configure):
+def configure_component(node, controller_manager_name, component_to_configure):
inactive_state = State()
inactive_state.id = State.PRIMARY_STATE_INACTIVE
inactive_state.label = "inactive"
- for component in components_to_configure:
- handle_set_component_state_service_call(
- node, controller_manager_name, component, inactive_state, "configured"
- )
+ handle_set_component_state_service_call(
+ node, controller_manager_name, component_to_configure, inactive_state, "configured"
+ )
def main(args=None):
rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
parser = argparse.ArgumentParser()
- activate_or_confiigure_grp = parser.add_mutually_exclusive_group(required=True)
+ activate_or_configure_grp = parser.add_mutually_exclusive_group(required=True)
parser.add_argument(
- "hardware_component_name",
- help="The name of the hardware component which should be activated.",
+ "hardware_component_names",
+ help="The name of the hardware components which should be activated.",
+ nargs="+",
)
parser.add_argument(
"-c",
@@ -126,13 +122,13 @@ def main(args=None):
type=float,
)
# add arguments which are mutually exclusive
- activate_or_confiigure_grp.add_argument(
+ activate_or_configure_grp.add_argument(
"--activate",
help="Activates the given components. Note: Components are by default configured before activated. ",
action="store_true",
required=False,
)
- activate_or_confiigure_grp.add_argument(
+ activate_or_configure_grp.add_argument(
"--configure",
help="Configures the given components.",
action="store_true",
@@ -141,9 +137,9 @@ def main(args=None):
command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
args = parser.parse_args(command_line_args)
+ hardware_components = args.hardware_component_names
controller_manager_name = args.controller_manager
controller_manager_timeout = args.controller_manager_timeout
- hardware_component = [args.hardware_component_name]
activate = args.activate
configure = args.configure
@@ -156,28 +152,27 @@ def main(args=None):
controller_manager_name = f"/{controller_manager_name}"
try:
- if not is_hardware_component_loaded(
- node, controller_manager_name, hardware_component, controller_manager_timeout
- ):
- node.get_logger().warn(
- bcolors.WARNING
- + "Hardware Component is not loaded - state can not be changed."
- + bcolors.ENDC
- )
- elif activate:
- activate_components(node, controller_manager_name, hardware_component)
- elif configure:
- configure_components(node, controller_manager_name, hardware_component)
- else:
- node.get_logger().error(
- 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag'
- )
- parser.print_help()
- return 0
+ for hardware_component in hardware_components:
+ if not is_hardware_component_loaded(
+ node, controller_manager_name, hardware_component, controller_manager_timeout
+ ):
+ node.get_logger().warn(
+ f"{bcolors.WARNING}Hardware Component is not loaded - state can not be changed.{bcolors.ENDC}"
+ )
+ elif activate:
+ activate_component(node, controller_manager_name, hardware_component)
+ elif configure:
+ configure_component(node, controller_manager_name, hardware_component)
+ else:
+ node.get_logger().error(
+ f'{bcolors.FAIL}You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag{bcolors.ENDC}'
+ )
+ parser.print_help()
+ return 0
except KeyboardInterrupt:
pass
except ServiceNotFoundError as err:
- node.get_logger().fatal(str(err))
+ node.get_logger().fatal(f"{bcolors.FAIL}{str(err)}{bcolors.ENDC}")
return 1
finally:
rclpy.shutdown()
diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py
index d6df3be01f..7e9fe3443b 100644
--- a/controller_manager/controller_manager/spawner.py
+++ b/controller_manager/controller_manager/spawner.py
@@ -112,7 +112,16 @@ def main(args=None):
"--controller-manager-timeout",
help="Time to wait for the controller manager",
required=False,
- default=0,
+ default=0.0,
+ type=float,
+ )
+ parser.add_argument(
+ "--switch-timeout",
+ help="Time to wait for a successful state switch of controllers."
+ " Useful when switching cannot be performed immediately, e.g.,"
+ " paused simulations at startup",
+ required=False,
+ default=5.0,
type=float,
)
parser.add_argument(
@@ -129,6 +138,7 @@ def main(args=None):
controller_manager_name = args.controller_manager
param_file = args.param_file
controller_manager_timeout = args.controller_manager_timeout
+ switch_timeout = args.switch_timeout
if param_file and not os.path.isfile(param_file):
raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
@@ -206,7 +216,13 @@ def main(args=None):
if not args.inactive and not args.activate_as_group:
ret = switch_controllers(
- node, controller_manager_name, [], [controller_name], True, True, 5.0
+ node,
+ controller_manager_name,
+ [],
+ [controller_name],
+ True,
+ True,
+ switch_timeout,
)
if not ret.ok:
node.get_logger().error(
@@ -224,7 +240,13 @@ def main(args=None):
if not args.inactive and args.activate_as_group:
ret = switch_controllers(
- node, controller_manager_name, [], controller_names, True, True, 5.0
+ node,
+ controller_manager_name,
+ [],
+ controller_names,
+ True,
+ True,
+ switch_timeout,
)
if not ret.ok:
node.get_logger().error(
@@ -250,7 +272,13 @@ def main(args=None):
node.get_logger().info("Interrupt captured, deactivating and unloading controller")
# TODO(saikishor) we might have an issue in future, if any of these controllers is in chained mode
ret = switch_controllers(
- node, controller_manager_name, controller_names, [], True, True, 5.0
+ node,
+ controller_manager_name,
+ controller_names,
+ [],
+ True,
+ True,
+ switch_timeout,
)
if not ret.ok:
node.get_logger().error(
diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py
index e42d85aee9..9e380f5086 100644
--- a/controller_manager/controller_manager/unspawner.py
+++ b/controller_manager/controller_manager/unspawner.py
@@ -36,17 +36,33 @@ def main(args=None):
default="/controller_manager",
required=False,
)
+ parser.add_argument(
+ "--switch-timeout",
+ help="Time to wait for a successful state switch of controllers."
+ " Useful when switching cannot be performed immediately, e.g.,"
+ " paused simulations at startup",
+ required=False,
+ default=5.0,
+ type=float,
+ )
command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
args = parser.parse_args(command_line_args)
controller_names = args.controller_names
controller_manager_name = args.controller_manager
+ switch_timeout = args.switch_timeout
node = Node("unspawner_" + controller_names[0])
try:
# Ignore returncode, because message is already printed and we'll try to unload anyway
ret = switch_controllers(
- node, controller_manager_name, controller_names, [], True, True, 5.0
+ node,
+ controller_manager_name,
+ controller_names,
+ [],
+ True,
+ True,
+ switch_timeout,
)
node.get_logger().info("Deactivated controller")
diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst
index d3dd5e90c8..ca222d68c0 100644
--- a/controller_manager/doc/userdoc.rst
+++ b/controller_manager/doc/userdoc.rst
@@ -157,9 +157,9 @@ There are two scripts to interact with controller manager from launch files:
.. code-block:: console
$ ros2 run controller_manager spawner -h
- usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-t CONTROLLER_TYPE] [-u]
- [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT]
- controller_name
+ usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT]
+ [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group]
+ controller_names [controller_names ...]
positional arguments:
controller_names List of controllers
@@ -177,27 +177,84 @@ There are two scripts to interact with controller manager from launch files:
-u, --unload-on-kill Wait until this application is interrupted and unload controller
--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT
Time to wait for the controller manager
+ --switch-timeout SWITCH_TIMEOUT
+ Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused
+ simulations at startup
--activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether
- --fallback_controllers FALLBACK_CONTROLLERS [FALLBACK_CONTROLLERS ...]
- Fallback controllers list are activated as a fallback strategy when the spawned controllers fail. When the argument is provided, it takes precedence over the fallback_controllers list in the
- param file
+The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files:
+
+ .. code-block:: yaml
+
+ /**/position_trajectory_controller:
+ ros__parameters:
+ type: joint_trajectory_controller/JointTrajectoryController
+ joints:
+ - joint1
+ - joint2
+
+ command_interfaces:
+ - position
+ .....
+
+ .. code-block:: yaml
+
+ /position_trajectory_controller:
+ ros__parameters:
+ type: joint_trajectory_controller/JointTrajectoryController
+ joints:
+ - joint1
+ - joint2
+
+ command_interfaces:
+ - position
+ .....
+
+ .. code-block:: yaml
+
+ position_trajectory_controller:
+ ros__parameters:
+ type: joint_trajectory_controller/JointTrajectoryController
+ joints:
+ - joint1
+ - joint2
+
+ command_interfaces:
+ - position
+ .....
+
+ .. code-block:: yaml
+
+ /rrbot_1/position_trajectory_controller:
+ ros__parameters:
+ type: joint_trajectory_controller/JointTrajectoryController
+ joints:
+ - joint1
+ - joint2
+
+ command_interfaces:
+ - position
+ .....
+
``unspawner``
^^^^^^^^^^^^^^^^
.. code-block:: console
$ ros2 run controller_manager unspawner -h
- usage: unspawner [-h] [-c CONTROLLER_MANAGER] controller_name
+ usage: unspawner [-h] [-c CONTROLLER_MANAGER] [--switch-timeout SWITCH_TIMEOUT] controller_names [controller_names ...]
positional arguments:
- controller_name Name of the controller
+ controller_names Name of the controller
- optional arguments:
+ options:
-h, --help show this help message and exit
-c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
Name of the controller manager ROS node
+ --switch-timeout SWITCH_TIMEOUT
+ Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused
+ simulations at startup
``hardware_spawner``
^^^^^^^^^^^^^^^^^^^^^^
@@ -205,16 +262,20 @@ There are two scripts to interact with controller manager from launch files:
.. code-block:: console
$ ros2 run controller_manager hardware_spawner -h
- usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name
+ usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT]
+ (--activate | --configure)
+ hardware_component_names [hardware_component_names ...]
positional arguments:
- hardware_component_name
- The name of the hardware component which should be activated.
+ hardware_component_names
+ The name of the hardware components which should be activated.
options:
-h, --help show this help message and exit
-c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
Name of the controller manager ROS node
+ --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT
+ Time to wait for the controller manager
--activate Activates the given components. Note: Components are by default configured before activated.
--configure Configures the given components.
diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
index 273b48b022..068eefc1f9 100644
--- a/controller_manager/include/controller_manager/controller_manager.hpp
+++ b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -22,7 +22,6 @@
#include
#include
-#include "controller_interface/async_controller.hpp"
#include "controller_interface/chainable_controller_interface.hpp"
#include "controller_interface/controller_interface.hpp"
#include "controller_interface/controller_interface_base.hpp"
@@ -124,7 +123,7 @@ class ControllerManager : public rclcpp::Node
controller_spec.c = controller;
controller_spec.info.name = controller_name;
controller_spec.info.type = controller_type;
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
return add_controller_impl(controller_spec);
}
@@ -628,9 +627,6 @@ class ControllerManager : public rclcpp::Node
};
SwitchParams switch_params_;
-
- std::unordered_map>
- async_controller_threads_;
};
} // namespace controller_manager
diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp
index 9ce1aab8b6..0f44867814 100644
--- a/controller_manager/include/controller_manager/controller_spec.hpp
+++ b/controller_manager/include/controller_manager/controller_spec.hpp
@@ -37,7 +37,7 @@ struct ControllerSpec
{
hardware_interface::ControllerInfo info;
controller_interface::ControllerInterfaceBaseSharedPtr c;
- std::shared_ptr next_update_cycle_time;
+ std::shared_ptr last_update_cycle_time;
};
struct ControllerChainSpec
diff --git a/controller_manager/package.xml b/controller_manager/package.xml
index 030fcd3f7a..18189d5d16 100644
--- a/controller_manager/package.xml
+++ b/controller_manager/package.xml
@@ -2,7 +2,7 @@
controller_manager
- 4.18.0
+ 4.20.0
Description of controller_manager
Bence Magyar
Denis Štogl
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index c4be6fb2fb..71ca554116 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -581,7 +581,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
controller_spec.c = controller;
controller_spec.info.name = controller_name;
controller_spec.info.type = controller_type;
- controller_spec.next_update_cycle_time = std::make_shared(
+ controller_spec.last_update_cycle_time = std::make_shared(
0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
// We have to fetch the parameters_file at the time of loading the controller, because this way we
@@ -687,13 +687,6 @@ controller_interface::return_type ControllerManager::unload_controller(
controller_name.c_str());
return controller_interface::return_type::ERROR;
}
- if (controller.c->is_async())
- {
- RCLCPP_DEBUG(
- get_logger(), "Removing controller '%s' from the list of async controllers",
- controller_name.c_str());
- async_controller_threads_.erase(controller_name);
- }
RCLCPP_DEBUG(get_logger(), "Cleanup controller");
controller_chain_spec_cleanup(controller_chain_spec_, controller_name);
@@ -836,14 +829,6 @@ controller_interface::return_type ControllerManager::configure_controller(
return controller_interface::return_type::ERROR;
}
- // ASYNCHRONOUS CONTROLLERS: Start background thread for update
- if (controller->is_async())
- {
- async_controller_threads_.emplace(
- controller_name,
- std::make_unique(controller, update_rate_));
- }
-
const auto controller_update_rate = controller->get_update_rate();
const auto cm_update_rate = get_update_rate();
if (controller_update_rate > cm_update_rate)
@@ -1413,6 +1398,18 @@ controller_interface::return_type ControllerManager::switch_controller(
RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str());
}
+ // wait for deactivating async controllers to finish their current cycle
+ for (const auto & controller : deactivate_request_)
+ {
+ auto controller_it = std::find_if(
+ controllers.begin(), controllers.end(),
+ std::bind(controller_name_compare, std::placeholders::_1, controller));
+ if (controller_it != controllers.end())
+ {
+ controller_it->c->wait_for_trigger_update_to_finish();
+ }
+ }
+
if (
!activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty())
{
@@ -1691,8 +1688,8 @@ void ControllerManager::activate_controllers(
continue;
}
auto controller = found_it->c;
- // reset the next update cycle time for newly activated controllers
- *found_it->next_update_cycle_time =
+ // reset the last update cycle time for newly activated controllers
+ *found_it->last_update_cycle_time =
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
bool assignment_successful = true;
@@ -1818,11 +1815,6 @@ void ControllerManager::activate_controllers(
resource_manager_->make_controller_exported_state_interfaces_available(controller_name);
resource_manager_->make_controller_reference_interfaces_available(controller_name);
}
-
- if (controller->is_async())
- {
- async_controller_threads_.at(controller_name)->activate();
- }
}
}
@@ -2363,29 +2355,50 @@ controller_interface::return_type ControllerManager::update(
{
// TODO(v-lopez) we could cache this information
// https://github.com/ros-controls/ros2_control/issues/153
- if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c))
+ if (is_controller_active(*loaded_controller.c))
{
+ if (
+ switch_params_.do_switch && loaded_controller.c->is_async() &&
+ std::find(
+ deactivate_request_.begin(), deactivate_request_.end(), loaded_controller.info.name) !=
+ deactivate_request_.end())
+ {
+ RCLCPP_DEBUG(
+ get_logger(), "Skipping update for async controller '%s' as it is being deactivated",
+ loaded_controller.info.name.c_str());
+ continue;
+ }
const auto controller_update_rate = loaded_controller.c->get_update_rate();
const bool run_controller_at_cm_rate = (controller_update_rate >= update_rate_);
const auto controller_period =
run_controller_at_cm_rate ? period
: rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
+ const rclcpp::Time current_time = get_clock()->now();
if (
- *loaded_controller.next_update_cycle_time ==
+ *loaded_controller.last_update_cycle_time ==
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
{
// it is zero after activation
+ *loaded_controller.last_update_cycle_time = current_time - controller_period;
RCLCPP_DEBUG(
- get_logger(), "Setting next_update_cycle_time to %fs for the controller : %s",
- time.seconds(), loaded_controller.info.name.c_str());
- *loaded_controller.next_update_cycle_time = time;
+ get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s",
+ loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str());
}
-
- bool controller_go =
+ const auto controller_actual_period =
+ (current_time - *loaded_controller.last_update_cycle_time);
+
+ /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the
+ /// jitter in the system sleep cycles.
+ // For instance, A controller running at 50 Hz and the CM running at 100Hz, then when we have
+ // an update cycle at 0.019s (ideally, the controller should only trigger >= 0.02s), if we
+ // wait for next cycle, then trigger will happen at ~0.029 sec and this is creating an issue
+ // to keep up with the controller update rate (see issue #1769).
+ const bool controller_go =
+ run_controller_at_cm_rate ||
(time ==
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
- (time.seconds() >= loaded_controller.next_update_cycle_time->seconds());
+ (controller_actual_period.seconds() * controller_update_rate >= 0.99);
RCLCPP_DEBUG(
get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
@@ -2394,13 +2407,13 @@ controller_interface::return_type ControllerManager::update(
if (controller_go)
{
- const auto controller_actual_period =
- (time - *loaded_controller.next_update_cycle_time) + controller_period;
auto controller_ret = controller_interface::return_type::OK;
+ bool trigger_status = true;
// Catch exceptions thrown by the controller update function
try
{
- controller_ret = loaded_controller.c->update(time, controller_actual_period);
+ std::tie(trigger_status, controller_ret) =
+ loaded_controller.c->trigger_update(time, controller_actual_period);
}
catch (const std::exception & e)
{
@@ -2417,7 +2430,7 @@ controller_interface::return_type ControllerManager::update(
controller_ret = controller_interface::return_type::ERROR;
}
- *loaded_controller.next_update_cycle_time += controller_period;
+ *loaded_controller.last_update_cycle_time = current_time;
if (controller_ret != controller_interface::return_type::OK)
{
@@ -2641,8 +2654,6 @@ unsigned int ControllerManager::get_update_rate() const { return update_rate_; }
void ControllerManager::shutdown_async_controllers_and_components()
{
- async_controller_threads_.erase(
- async_controller_threads_.begin(), async_controller_threads_.end());
resource_manager_->shutdown_async_components();
}
diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
index e0094b7e01..f8347df952 100644
--- a/controller_manager/src/ros2_control_node.cpp
+++ b/controller_manager/src/ros2_control_node.cpp
@@ -20,7 +20,7 @@
#include "controller_manager/controller_manager.hpp"
#include "rclcpp/executors.hpp"
-#include "realtime_tools/thread_priority.hpp"
+#include "realtime_tools/realtime_helpers.hpp"
using namespace std::chrono_literals;
@@ -57,12 +57,36 @@ int main(int argc, char ** argv)
auto cm = std::make_shared(
executor, manager_node_name, "", cm_node_options);
+ const bool use_sim_time = cm->get_parameter_or("use_sim_time", false);
+
+ const bool lock_memory = cm->get_parameter_or("lock_memory", true);
+ std::string message;
+ if (lock_memory && !realtime_tools::lock_memory(message))
+ {
+ RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str());
+ }
+
+ const int cpu_affinity = cm->get_parameter_or("cpu_affinity", -1);
+ if (cpu_affinity >= 0)
+ {
+ const auto affinity_result = realtime_tools::set_current_thread_affinity(cpu_affinity);
+ if (!affinity_result.first)
+ {
+ RCLCPP_WARN(
+ cm->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str());
+ }
+ }
+
RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
+ const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority);
+ RCLCPP_INFO(
+ cm->get_logger(), "Spawning %s RT thread with scheduler priority: %d", cm->get_name(),
+ thread_priority);
std::thread cm_thread(
- [cm]()
+ [cm, thread_priority, use_sim_time]()
{
- if (!realtime_tools::configure_sched_fifo(kSchedPriority))
+ if (!realtime_tools::configure_sched_fifo(thread_priority))
{
RCLCPP_WARN(
cm->get_logger(),
@@ -75,7 +99,7 @@ int main(int argc, char ** argv)
{
RCLCPP_INFO(
cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.",
- kSchedPriority);
+ thread_priority);
}
// for calculating sleep time
@@ -101,7 +125,14 @@ int main(int argc, char ** argv)
// wait until we hit the end of the period
next_iteration_time += period;
- std::this_thread::sleep_until(next_iteration_time);
+ if (use_sim_time)
+ {
+ cm->get_clock()->sleep_until(current_time + period);
+ }
+ else
+ {
+ std::this_thread::sleep_until(next_iteration_time);
+ }
}
cm->shutdown_async_controllers_and_components();
diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp
index ac89239e09..04ae8c02c2 100644
--- a/controller_manager/test/test_controller/test_controller.cpp
+++ b/controller_manager/test/test_controller/test_controller.cpp
@@ -61,6 +61,10 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con
controller_interface::return_type TestController::update(
const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
+ if (is_async())
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000 / (2 * get_update_rate())));
+ }
update_period_ = period;
++internal_counter;
diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
index a016b20440..0c4e51985f 100644
--- a/controller_manager/test/test_controller_manager.cpp
+++ b/controller_manager/test/test_controller_manager.cpp
@@ -233,6 +233,191 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
EXPECT_EQ(1, test_controller.use_count());
}
+TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle)
+{
+ const auto test_param = GetParam();
+ auto test_controller = std::make_shared();
+ auto test_controller2 = std::make_shared();
+ constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name";
+ cm_->add_controller(
+ test_controller, test_controller::TEST_CONTROLLER_NAME,
+ test_controller::TEST_CONTROLLER_CLASS_NAME);
+ cm_->add_controller(
+ test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
+ EXPECT_EQ(2, test_controller.use_count());
+
+ // setup interface to claim from controllers
+ controller_interface::InterfaceConfiguration cmd_itfs_cfg;
+ cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+ for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
+ {
+ cmd_itfs_cfg.names.push_back(interface);
+ }
+ test_controller->set_command_interface_configuration(cmd_itfs_cfg);
+
+ controller_interface::InterfaceConfiguration state_itfs_cfg;
+ state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+ for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
+ {
+ state_itfs_cfg.names.push_back(interface);
+ }
+ for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
+ {
+ state_itfs_cfg.names.push_back(interface);
+ }
+ test_controller->set_state_interface_configuration(state_itfs_cfg);
+
+ controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
+ cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+ for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES)
+ {
+ cmd_itfs_cfg2.names.push_back(interface);
+ }
+ test_controller2->set_command_interface_configuration(cmd_itfs_cfg2);
+
+ controller_interface::InterfaceConfiguration state_itfs_cfg2;
+ state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
+ test_controller2->set_state_interface_configuration(state_itfs_cfg2);
+
+ // Check if namespace is set correctly
+ RCLCPP_INFO(
+ rclcpp::get_logger("test_controller_manager"), "Controller Manager namespace is '%s'",
+ cm_->get_namespace());
+ EXPECT_STREQ(cm_->get_namespace(), "/");
+ RCLCPP_INFO(
+ rclcpp::get_logger("test_controller_manager"), "Controller 1 namespace is '%s'",
+ test_controller->get_node()->get_namespace());
+ EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/");
+ RCLCPP_INFO(
+ rclcpp::get_logger("test_controller_manager"), "Controller 2 namespace is '%s'",
+ test_controller2->get_node()->get_namespace());
+ EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/");
+
+ EXPECT_EQ(
+ controller_interface::return_type::OK,
+ cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+ EXPECT_EQ(0u, test_controller->internal_counter)
+ << "Update should not reach an unconfigured controller";
+
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+ test_controller->get_lifecycle_state().id());
+
+ // configure controller
+ rclcpp::Parameter update_rate_parameter("update_rate", static_cast(20));
+ rclcpp::Parameter is_async_parameter("is_async", rclcpp::ParameterValue(true));
+ test_controller->get_node()->set_parameter(update_rate_parameter);
+ test_controller->get_node()->set_parameter(is_async_parameter);
+ {
+ ControllerManagerRunner cm_runner(this);
+ cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+ cm_->configure_controller(TEST_CONTROLLER2_NAME);
+ }
+ EXPECT_EQ(
+ controller_interface::return_type::OK,
+ cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+ EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
+ EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started";
+
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+ test_controller->get_lifecycle_state().id());
+
+ // Start controller, will take effect at the end of the update function
+ std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME};
+ std::vector stop_controllers = {};
+ auto switch_future = std::async(
+ std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+ start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));
+
+ EXPECT_EQ(
+ controller_interface::return_type::OK,
+ cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+ EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update";
+ {
+ ControllerManagerRunner cm_runner(this);
+ EXPECT_EQ(test_param.expected_return, switch_future.get());
+ }
+
+ EXPECT_EQ(
+ controller_interface::return_type::OK,
+ cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+ EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter);
+
+ // Start the real test controller, will take effect at the end of the update function
+ start_controllers = {test_controller::TEST_CONTROLLER_NAME};
+ stop_controllers = {};
+ switch_future = std::async(
+ std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+ start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));
+
+ ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+ << "switch_controller should be blocking until next update cycle";
+
+ EXPECT_EQ(
+ controller_interface::return_type::OK,
+ cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+ EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
+ {
+ ControllerManagerRunner cm_runner(this);
+ EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+ }
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
+
+ EXPECT_EQ(
+ controller_interface::return_type::OK,
+ cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+ EXPECT_EQ(test_controller->internal_counter, 0u);
+ std::this_thread::sleep_for(
+ std::chrono::milliseconds(1000 / (test_controller->get_update_rate())));
+ EXPECT_EQ(test_controller->internal_counter, 1u);
+ size_t last_internal_counter = test_controller->internal_counter;
+
+ // Stop controller, will take effect at the end of the update function
+ start_controllers = {};
+ stop_controllers = {test_controller::TEST_CONTROLLER_NAME};
+ switch_future = std::async(
+ std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+ start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));
+
+ ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+ << "switch_controller should be blocking until next update cycle";
+
+ EXPECT_EQ(
+ controller_interface::return_type::OK,
+ cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+ EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
+ << "This shouldn't have updated as this is async and in the controller it is waiting before "
+ "updating the counter";
+ std::this_thread::sleep_for(
+ std::chrono::milliseconds(1000 / (test_controller->get_update_rate())));
+ EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
+ << "Controller is stopped at the end of update, so it should have done one more update";
+ {
+ ControllerManagerRunner cm_runner(this);
+ EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+ }
+
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+ test_controller->get_lifecycle_state().id());
+ auto unload_future = std::async(
+ std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_,
+ test_controller::TEST_CONTROLLER_NAME);
+
+ ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100)))
+ << "unload_controller should be blocking until next update cycle";
+ ControllerManagerRunner cm_runner(this);
+ EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());
+
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+ test_controller->get_lifecycle_state().id());
+ EXPECT_EQ(1, test_controller.use_count());
+}
+
TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
{
auto strictness = GetParam().strictness;
@@ -390,10 +575,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
// In case of a non perfect divisor, the update period should respect the rule
// [cm_update_rate, 2*cm_update_rate)
EXPECT_THAT(
- test_controller->update_period_,
- testing::AllOf(
- testing::Ge(rclcpp::Duration::from_seconds(1.0 / cm_update_rate)),
- testing::Lt(rclcpp::Duration::from_seconds(2.0 / cm_update_rate))));
+ test_controller->update_period_.seconds(),
+ testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate))));
loop_rate.sleep();
}
// if we do 2 times of the controller_manager update rate, the internal counter should be
@@ -455,6 +638,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
ControllerManagerRunner cm_runner(this);
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
}
+ time_ = test_controller->get_node()->now(); // set to something nonzero
+ cm_->get_clock()->sleep_until(time_ + PERIOD);
+ time_ = cm_->get_clock()->now();
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -465,7 +651,6 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
test_controller->get_lifecycle_state().id());
// Start controller, will take effect at the end of the update function
- time_ = test_controller->get_node()->now(); // set to something nonzero
const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME};
std::vector stop_controllers = {};
@@ -476,7 +661,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";
- time_ += rclcpp::Duration::from_seconds(0.01);
+ cm_->get_clock()->sleep_until(time_ + PERIOD);
+ time_ = cm_->get_clock()->now();
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -492,25 +678,29 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
const auto cm_update_rate = cm_->get_update_rate();
const auto controller_update_rate = test_controller->get_update_rate();
+ const double controller_period = 1.0 / controller_update_rate;
const auto initial_counter = test_controller->internal_counter;
// don't start with zero to check if the period is correct if controller is activated anytime
rclcpp::Time time = time_;
for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
{
+ rclcpp::Time old_time = time;
+ cm_->get_clock()->sleep_until(old_time + PERIOD);
+ time = cm_->get_clock()->now();
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time, rclcpp::Duration::from_seconds(0.01)));
// In case of a non perfect divisor, the update period should respect the rule
// [controller_update_rate, 2*controller_update_rate)
EXPECT_THAT(
- test_controller->update_period_,
+ test_controller->update_period_.seconds(),
testing::AllOf(
- testing::Ge(rclcpp::Duration::from_seconds(1.0 / controller_update_rate)),
- testing::Lt(rclcpp::Duration::from_seconds(2.0 / controller_update_rate))))
- << "update_counter: " << update_counter;
+ testing::Gt(0.99 * controller_period),
+ testing::Lt((1.05 * controller_period) + PERIOD.seconds())))
+ << "update_counter: " << update_counter << " desired controller period: " << controller_period
+ << " actual controller period: " << test_controller->update_period_.seconds();
- time += rclcpp::Duration::from_seconds(0.01);
if (update_counter % cm_update_rate == 0)
{
const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate;
@@ -523,15 +713,15 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
EXPECT_THAT(
test_controller->internal_counter - initial_counter,
testing::AnyOf(
- testing::Eq(controller_update_rate * no_of_secs_passed),
- testing::Eq((controller_update_rate * no_of_secs_passed) - 1)));
+ testing::Ge((controller_update_rate - 1) * no_of_secs_passed),
+ testing::Lt((controller_update_rate * no_of_secs_passed))));
}
}
}
INSTANTIATE_TEST_SUITE_P(
per_controller_update_rate_check, TestControllerUpdateRates,
- testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98));
+ testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 90));
class TestControllerManagerFallbackControllers
: public ControllerManagerFixture,
@@ -579,7 +769,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_contro
controller_spec.info.name = test_controller_1_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {test_controller_2_name};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
ControllerManagerRunner cm_runner(this);
cm_->add_controller(controller_spec); // add controller_1
@@ -587,7 +777,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_contro
controller_spec.info.name = test_controller_2_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_2
}
EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
@@ -661,7 +851,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activ
controller_spec.info.name = test_controller_1_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {test_controller_2_name};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
ControllerManagerRunner cm_runner(this);
cm_->add_controller(controller_spec); // add controller_1
@@ -669,7 +859,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activ
controller_spec.info.name = test_controller_2_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_2
}
EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
@@ -759,7 +949,7 @@ TEST_F(
controller_spec.info.name = test_controller_1_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {test_controller_2_name};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
ControllerManagerRunner cm_runner(this);
cm_->add_controller(controller_spec); // add controller_1
@@ -767,7 +957,7 @@ TEST_F(
controller_spec.info.name = test_controller_2_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_2
}
EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
@@ -848,7 +1038,7 @@ TEST_F(
controller_spec.info.name = test_controller_1_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {test_controller_2_name};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
ControllerManagerRunner cm_runner(this);
cm_->add_controller(controller_spec); // add controller_1
@@ -856,7 +1046,7 @@ TEST_F(
controller_spec.info.name = test_controller_2_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_2
}
EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
@@ -944,7 +1134,7 @@ TEST_F(
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {
test_controller_2_name, test_controller_3_name};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
ControllerManagerRunner cm_runner(this);
cm_->add_controller(controller_spec); // add controller_1
@@ -952,14 +1142,14 @@ TEST_F(
controller_spec.info.name = test_controller_2_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_2
controller_spec.c = test_controller_3;
controller_spec.info.name = test_controller_3_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_3
}
@@ -1094,7 +1284,7 @@ TEST_F(
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {
test_controller_2_name, test_controller_3_name};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
ControllerManagerRunner cm_runner(this);
cm_->add_controller(controller_spec); // add controller_1
@@ -1102,21 +1292,21 @@ TEST_F(
controller_spec.info.name = test_controller_2_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_2
controller_spec.c = test_controller_3;
controller_spec.info.name = test_controller_3_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_3
controller_spec.c = test_controller_4;
controller_spec.info.name = test_controller_4_name;
controller_spec.info.type = "test_chainable_controller::TestChainableController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_4
}
@@ -1196,7 +1386,7 @@ TEST_F(
controller_spec.info.name = test_controller_1_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {test_controller_3_name};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_1
EXPECT_EQ(
@@ -1228,7 +1418,7 @@ TEST_F(
// available
controller_spec.info.fallback_controllers_names = {
test_controller_4_name, test_controller_3_name, test_controller_2_name};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_1
EXPECT_EQ(
@@ -1324,7 +1514,7 @@ TEST_F(
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {
test_controller_2_name, test_controller_4_name};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
ControllerManagerRunner cm_runner(this);
cm_->add_controller(controller_spec); // add controller_1
@@ -1332,21 +1522,21 @@ TEST_F(
controller_spec.info.name = test_controller_2_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_2
controller_spec.c = test_controller_3;
controller_spec.info.name = test_controller_3_name;
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_3
controller_spec.c = test_controller_4;
controller_spec.info.name = test_controller_4_name;
controller_spec.info.type = "test_chainable_controller::TestChainableController";
controller_spec.info.fallback_controllers_names = {};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_4
}
@@ -1432,7 +1622,7 @@ TEST_F(
controller_spec.info.type = "test_controller::TestController";
controller_spec.info.fallback_controllers_names = {
test_controller_3_name, test_controller_4_name};
- controller_spec.next_update_cycle_time = std::make_shared(0);
+ controller_spec.last_update_cycle_time = std::make_shared(0);
cm_->add_controller(controller_spec); // add controller_1
EXPECT_EQ(
diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml
index 892427bab7..087994bd23 100644
--- a/controller_manager/test/test_controller_spawner_with_type.yaml
+++ b/controller_manager/test/test_controller_spawner_with_type.yaml
@@ -3,25 +3,40 @@ ctrl_with_parameters_and_type:
type: "controller_manager/test_controller"
joint_names: ["joint0"]
-chainable_ctrl_with_parameters_and_type:
- ros__parameters:
- type: "controller_manager/test_chainable_controller"
- joint_names: ["joint1"]
+/**:
+ chainable_ctrl_with_parameters_and_type:
+ ros__parameters:
+ type: "controller_manager/test_chainable_controller"
+ joint_names: ["joint1"]
+
+ wildcard_ctrl_with_parameters_and_type:
+ ros__parameters:
+ type: "controller_manager/test_controller"
+ joint_names: ["joint1"]
ctrl_with_parameters_and_no_type:
ros__parameters:
joint_names: ["joint2"]
-/foo_namespace/ctrl_with_parameters_and_type:
+/foo_namespace/ns_ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_controller"
joint_names: ["joint1"]
-/foo_namespace/chainable_ctrl_with_parameters_and_type:
+/foo_namespace/ns_chainable_ctrl_with_parameters_and_type:
+ ros__parameters:
+ type: "controller_manager/test_chainable_controller"
+ joint_names: ["joint1"]
+
+/foo_namespace/ns_ctrl_with_parameters_and_no_type:
+ ros__parameters:
+ joint_names: ["joint2"]
+
+/**/wildcard_chainable_ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_chainable_controller"
joint_names: ["joint1"]
-/foo_namespace/ctrl_with_parameters_and_no_type:
+/**/wildcard_ctrl_with_parameters_and_no_type:
ros__parameters:
joint_names: ["joint2"]
diff --git a/controller_manager/test/test_hardware_spawner.cpp b/controller_manager/test/test_hardware_spawner.cpp
new file mode 100644
index 0000000000..6fd1269fa9
--- /dev/null
+++ b/controller_manager/test/test_hardware_spawner.cpp
@@ -0,0 +1,282 @@
+// Copyright 2021 PAL Robotics S.L.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include
+
+#include
+#include
+#include
+#include
+
+#include "controller_manager/controller_manager.hpp"
+#include "controller_manager_test_common.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "test_chainable_controller/test_chainable_controller.hpp"
+#include "test_controller/test_controller.hpp"
+
+using ::testing::_;
+using ::testing::Return;
+
+class RMServiceCaller
+{
+public:
+ explicit RMServiceCaller(const std::string & cm_name)
+ {
+ list_srv_node_ = std::make_shared("list_srv_client");
+ srv_executor_.add_node(list_srv_node_);
+ list_hw_components_client_ =
+ list_srv_node_->create_client(
+ cm_name + "/list_hardware_components");
+ }
+
+ lifecycle_msgs::msg::State get_component_state(const std::string & component_name)
+ {
+ auto request =
+ std::make_shared();
+ EXPECT_TRUE(list_hw_components_client_->wait_for_service(std::chrono::milliseconds(500)));
+ auto future = list_hw_components_client_->async_send_request(request);
+ EXPECT_EQ(srv_executor_.spin_until_future_complete(future), rclcpp::FutureReturnCode::SUCCESS);
+ auto result = future.get();
+
+ auto it = std::find_if(
+ std::begin(result->component), std::end(result->component),
+ [&component_name](const auto & cmp) { return cmp.name == component_name; });
+
+ EXPECT_NE(it, std::end(result->component));
+
+ return it->state;
+ };
+
+protected:
+ rclcpp::executors::SingleThreadedExecutor srv_executor_;
+ rclcpp::Node::SharedPtr list_srv_node_;
+ rclcpp::Client::SharedPtr
+ list_hw_components_client_;
+};
+
+using namespace std::chrono_literals;
+class TestHardwareSpawner : public ControllerManagerFixture,
+ public RMServiceCaller
+{
+public:
+ TestHardwareSpawner()
+ : ControllerManagerFixture(), RMServiceCaller(TEST_CM_NAME)
+ {
+ cm_->set_parameter(
+ rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware"));
+ }
+
+ void SetUp() override
+ {
+ ControllerManagerFixture::SetUp();
+
+ update_executor_ =
+ std::make_shared(rclcpp::ExecutorOptions(), 2);
+
+ update_executor_->add_node(cm_);
+ update_executor_spin_future_ =
+ std::async(std::launch::async, [this]() -> void { update_executor_->spin(); });
+ // This sleep is needed to prevent a too fast test from ending before the
+ // executor has began to spin, which causes it to hang
+ std::this_thread::sleep_for(50ms);
+ }
+
+ void TearDown() override { update_executor_->cancel(); }
+
+protected:
+ // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
+ std::shared_ptr update_executor_;
+ std::future update_executor_spin_future_;
+};
+
+int call_spawner(const std::string extra_args)
+{
+ std::string spawner_script =
+ "python3 -m coverage run --append --branch $(ros2 pkg prefix "
+ "controller_manager)/lib/controller_manager/hardware_spawner ";
+ return std::system((spawner_script + extra_args).c_str());
+}
+
+TEST_F(TestHardwareSpawner, spawner_with_no_arguments_errors)
+{
+ EXPECT_NE(call_spawner(""), 0) << "Missing mandatory arguments";
+}
+
+TEST_F(TestHardwareSpawner, spawner_without_manager_errors_with_given_timeout)
+{
+ EXPECT_NE(call_spawner("TestSystemHardware --controller-manager-timeout 1.0"), 0)
+ << "Wrong controller manager name";
+}
+
+TEST_F(TestHardwareSpawner, spawner_without_component_name_argument)
+{
+ EXPECT_NE(call_spawner("-c test_controller_manager"), 0)
+ << "Missing component name argument parameter";
+}
+
+TEST_F(TestHardwareSpawner, spawner_non_exising_hardware_component)
+{
+ EXPECT_NE(call_spawner("TestSystemHardware1 -c test_controller_manager"), 0)
+ << "Missing component name argument parameter";
+}
+
+TEST_F(TestHardwareSpawner, set_component_to_configured_state_and_back_to_activated)
+{
+ EXPECT_EQ(call_spawner("TestSystemHardware --configure -c test_controller_manager"), 0);
+ EXPECT_EQ(
+ get_component_state("TestSystemHardware").id,
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+
+ EXPECT_EQ(call_spawner("TestSystemHardware --activate -c test_controller_manager"), 0);
+ EXPECT_EQ(
+ get_component_state("TestSystemHardware").id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+}
+
+class TestHardwareSpawnerWithoutRobotDescription
+: public ControllerManagerFixture,
+ public RMServiceCaller
+{
+public:
+ TestHardwareSpawnerWithoutRobotDescription()
+ : ControllerManagerFixture(""),
+ RMServiceCaller(TEST_CM_NAME)
+ {
+ cm_->set_parameter(rclcpp::Parameter(
+ "hardware_components_initial_state.unconfigured",
+ std::vector{"TestSystemHardware"}));
+ }
+
+public:
+ void SetUp() override
+ {
+ ControllerManagerFixture::SetUp();
+
+ update_timer_ = cm_->create_wall_timer(
+ std::chrono::milliseconds(10),
+ [&]()
+ {
+ cm_->read(time_, PERIOD);
+ cm_->update(time_, PERIOD);
+ cm_->write(time_, PERIOD);
+ });
+
+ update_executor_ =
+ std::make_shared(rclcpp::ExecutorOptions(), 2);
+
+ update_executor_->add_node(cm_);
+ update_executor_spin_future_ =
+ std::async(std::launch::async, [this]() -> void { update_executor_->spin(); });
+ // This sleep is needed to prevent a too fast test from ending before the
+ // executor has began to spin, which causes it to hang
+ std::this_thread::sleep_for(50ms);
+ }
+
+ void TearDown() override { update_executor_->cancel(); }
+
+ rclcpp::TimerBase::SharedPtr robot_description_sending_timer_;
+
+protected:
+ rclcpp::TimerBase::SharedPtr update_timer_;
+
+ // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
+ std::shared_ptr update_executor_;
+ std::future update_executor_spin_future_;
+};
+
+TEST_F(TestHardwareSpawnerWithoutRobotDescription, when_no_robot_description_spawner_times_out)
+{
+ EXPECT_EQ(
+ call_spawner(
+ "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"),
+ 256)
+ << "could not change hardware state because not robot description and not services for "
+ "controller "
+ "manager are active";
+}
+
+TEST_F(TestHardwareSpawnerWithoutRobotDescription, spawner_with_later_load_of_robot_description)
+{
+ // Delay sending robot description
+ robot_description_sending_timer_ = cm_->create_wall_timer(
+ std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); });
+
+ EXPECT_EQ(
+ call_spawner(
+ "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"),
+ 256)
+ << "could not activate control because not robot description";
+ EXPECT_EQ(
+ call_spawner(
+ "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 2.5"),
+ 0);
+ EXPECT_EQ(
+ get_component_state("TestSystemHardware").id,
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+}
+
+class TestHardwareSpawnerWithNamespacedCM
+: public ControllerManagerFixture,
+ public RMServiceCaller
+{
+public:
+ TestHardwareSpawnerWithNamespacedCM()
+ : ControllerManagerFixture(
+ ros2_control_test_assets::minimal_robot_urdf, "foo_namespace"),
+ RMServiceCaller("foo_namespace/" + std::string(TEST_CM_NAME))
+ {
+ cm_->set_parameter(
+ rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware"));
+ }
+
+public:
+ void SetUp() override
+ {
+ ControllerManagerFixture::SetUp();
+
+ update_executor_ =
+ std::make_shared(rclcpp::ExecutorOptions(), 2);
+
+ update_executor_->add_node(cm_);
+ update_executor_spin_future_ =
+ std::async(std::launch::async, [this]() -> void { update_executor_->spin(); });
+ // This sleep is needed to prevent a too fast test from ending before the
+ // executor has began to spin, which causes it to hang
+ std::this_thread::sleep_for(50ms);
+ }
+
+ void TearDown() override { update_executor_->cancel(); }
+
+protected:
+ // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
+ std::shared_ptr update_executor_;
+ std::future update_executor_spin_future_;
+};
+
+TEST_F(TestHardwareSpawnerWithNamespacedCM, set_component_to_configured_state_cm_namespace)
+{
+ ControllerManagerRunner cm_runner(this);
+ EXPECT_EQ(
+ call_spawner(
+ "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"),
+ 256)
+ << "Should fail without defining the namespace";
+ EXPECT_EQ(
+ call_spawner("TestSystemHardware --configure -c test_controller_manager --ros-args -r "
+ "__ns:=/foo_namespace"),
+ 0);
+
+ EXPECT_EQ(
+ get_component_state("TestSystemHardware").id,
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+}
diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp
index cba832f8bf..74e1efeeed 100644
--- a/controller_manager/test/test_spawner_unspawner.cpp
+++ b/controller_manager/test/test_spawner_unspawner.cpp
@@ -674,14 +674,14 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)
// Provide controller type via the parsed file
EXPECT_EQ(
call_spawner(
- "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
+ "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --controller-manager-timeout 1.0 -p " +
test_file_path),
256)
<< "Should fail without the namespacing it";
EXPECT_EQ(
call_spawner(
- "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
+ "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager -p " +
test_file_path + " --ros-args -r __ns:=/foo_namespace"),
0);
@@ -689,17 +689,18 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)
ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0];
- ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type");
+ ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(
- cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);
+ cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(),
+ test_file_path);
auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
ASSERT_EQ(
- chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type");
+ chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.info.type,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
@@ -707,12 +708,12 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)
chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(
- cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(),
+ cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(),
test_file_path);
EXPECT_EQ(
call_spawner(
- "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path +
+ "ns_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path +
" --ros-args -r __ns:=/foo_namespace"),
256)
<< "Should fail as no type is defined!";
@@ -720,21 +721,18 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file)
ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
auto ctrl_1 = cm_->get_loaded_controllers()[0];
- ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type");
+ ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
- ASSERT_EQ(
- cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);
+ ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path);
auto ctrl_2 = cm_->get_loaded_controllers()[1];
- ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type");
+ ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
- ASSERT_EQ(
- cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(),
- test_file_path);
+ ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path);
}
TEST_F(
@@ -747,28 +745,28 @@ TEST_F(
// Provide controller type via the parsed file
EXPECT_EQ(
call_spawner(
- "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
+ "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --controller-manager-timeout 1.0 -p " +
test_file_path),
256)
<< "Should fail without the namespacing it";
EXPECT_EQ(
call_spawner(
- "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
+ "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p " +
test_file_path + " --ros-args -r __ns:=/random_namespace"),
256)
<< "Should fail when parsed namespace through both way with different namespaces";
EXPECT_EQ(
call_spawner(
- "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
+ "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p" +
test_file_path + " --ros-args -r __ns:=/foo_namespace"),
256)
<< "Should fail when parsed namespace through both ways even with same namespacing name";
EXPECT_EQ(
call_spawner(
- "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c "
+ "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c "
"test_controller_manager --namespace foo_namespace -p " +
test_file_path),
0)
@@ -777,17 +775,18 @@ TEST_F(
ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0];
- ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type");
+ ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(
- cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);
+ cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(),
+ test_file_path);
auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
ASSERT_EQ(
- chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type");
+ chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.info.type,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
@@ -795,12 +794,13 @@ TEST_F(
chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(
- cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(),
+ cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(),
test_file_path);
EXPECT_EQ(
call_spawner(
- "ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace -p " +
+ "ns_ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace "
+ "-p " +
test_file_path),
256)
<< "Should fail as no type is defined!";
@@ -808,19 +808,123 @@ TEST_F(
ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
auto ctrl_1 = cm_->get_loaded_controllers()[0];
- ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type");
+ ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path);
+
+ auto ctrl_2 = cm_->get_loaded_controllers()[1];
+ ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type");
+ ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
- cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path);
+ ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path);
+}
+
+TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file)
+{
+ const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_type.yaml";
+
+ ControllerManagerRunner cm_runner(this);
+ // Provide controller type via the parsed file
+ EXPECT_EQ(
+ call_spawner(
+ "wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type "
+ "--load-only -c "
+ "test_controller_manager --controller-manager-timeout 1.0 -p " +
+ test_file_path),
+ 256)
+ << "Should fail without the namespacing it due to timeout but can find the parameters";
+ EXPECT_EQ(
+ call_spawner(
+ "wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type "
+ "--load-only -c "
+ "test_controller_manager -p " +
+ test_file_path + " --ros-args -r __ns:=/foo_namespace"),
+ 0);
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
+
+ auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0];
+ ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "wildcard_ctrl_with_parameters_and_type");
+ ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_EQ(
+ ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+ auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
+ ASSERT_EQ(
+ chain_ctrl_with_parameters_and_type.info.name,
+ "wildcard_chainable_ctrl_with_parameters_and_type");
+ ASSERT_EQ(
+ chain_ctrl_with_parameters_and_type.info.type,
+ test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_EQ(
+ chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+ EXPECT_EQ(
+ call_spawner(
+ "wildcard_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path +
+ " --ros-args -r __ns:=/foo_namespace"),
+ 256)
+ << "Should fail as no type is defined!";
+ // Will still be same as the current call will fail
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
+
+ auto ctrl_1 = cm_->get_loaded_controllers()[0];
+ ASSERT_EQ(ctrl_1.info.name, "wildcard_ctrl_with_parameters_and_type");
+ ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_EQ(
+ ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
auto ctrl_2 = cm_->get_loaded_controllers()[1];
- ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type");
+ ASSERT_EQ(ctrl_2.info.name, "wildcard_chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+}
+
+TEST_F(
+ TestLoadControllerWithNamespacedCM,
+ spawner_test_fail_namespaced_controllers_with_non_wildcard_entries)
+{
+ const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_type.yaml";
+
+ ControllerManagerRunner cm_runner(this);
+ // Provide controller type via the parsed file
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_with_parameters_and_type --load-only -c "
+ "test_controller_manager --controller-manager-timeout 1.0 -p " +
+ test_file_path),
+ 256)
+ << "Should fail without the namespacing it";
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_with_parameters_and_type --load-only -c "
+ "test_controller_manager --namespace foo_namespace -p " +
+ test_file_path),
+ 256)
+ << "Should fail even namespacing it as ctrl_with_parameters_and_type is not a wildcard entry";
+ EXPECT_EQ(
+ call_spawner(
+ "chainable_ctrl_with_parameters_and_type --load-only -c "
+ "test_controller_manager --namespace foo_namespace -p " +
+ test_file_path),
+ 0)
+ << "Should work as chainable_ctrl_with_parameters_and_type is a wildcard entry";
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
+
+ auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0];
+ ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(
- cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(),
- test_file_path);
+ ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_EQ(
+ ctrl_with_parameters_and_type.c->get_lifecycle_state().id(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}
diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst
index 6b5007c6a9..46ecc7ab6e 100644
--- a/controller_manager_msgs/CHANGELOG.rst
+++ b/controller_manager_msgs/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package controller_manager_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.20.0 (2024-11-08)
+-------------------
+
+4.19.0 (2024-10-26)
+-------------------
+
4.18.0 (2024-10-07)
-------------------
diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml
index e733b3921b..78a9f99591 100644
--- a/controller_manager_msgs/package.xml
+++ b/controller_manager_msgs/package.xml
@@ -2,7 +2,7 @@
controller_manager_msgs
- 4.18.0
+ 4.20.0
Messages and services for the controller manager.
Bence Magyar
Denis Štogl
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
index 7fa7ae7f29..09a9236d79 100644
--- a/doc/release_notes.rst
+++ b/doc/release_notes.rst
@@ -75,6 +75,12 @@ controller_manager
* The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_).
* The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_).
* The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_).
+* Added support for the wildcard entries for the controller configuration files (`#1724 `_).
+* ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_).
+* ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_).
+* The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_).
+* The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_).
+* The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_).
hardware_interface
******************
@@ -108,6 +114,7 @@ hardware_interface
* Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_)
* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles.
* With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file.
+* With (`#1763 `_) parsing for SDF published to ``robot_description`` topic is now also supported.
joint_limits
************
diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst
index b21be43f60..17e276bc93 100644
--- a/hardware_interface/CHANGELOG.rst
+++ b/hardware_interface/CHANGELOG.rst
@@ -2,6 +2,17 @@
Changelog for package hardware_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.20.0 (2024-11-08)
+-------------------
+* Add Support for SDF (`#1763 `_)
+* [HW_IF] Prepare the handles for async operations (`#1750 `_)
+* Contributors: Aarav Gupta, Sai Kishor Kothakota
+
+4.19.0 (2024-10-26)
+-------------------
+* [RM/HW] Constify the exported state interfaces using ConstSharedPtr (`#1767 `_)
+* Contributors: Sai Kishor Kothakota
+
4.18.0 (2024-10-07)
-------------------
* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_)
diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp
index 4cdd81b60f..8aa214e728 100644
--- a/hardware_interface/include/hardware_interface/actuator_interface.hpp
+++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp
@@ -123,39 +123,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
{
info_ = hardware_info;
- import_state_interface_descriptions(info_);
- import_command_interface_descriptions(info_);
+ parse_state_interface_descriptions(info_.joints, joint_state_interfaces_);
+ parse_command_interface_descriptions(info_.joints, joint_command_interfaces_);
return CallbackReturn::SUCCESS;
};
- /**
- * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo.
- * Separate them into the possible types: Joint and store them.
- */
- virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info)
- {
- auto joint_state_interface_descriptions =
- parse_state_interface_descriptions(hardware_info.joints);
- for (const auto & description : joint_state_interface_descriptions)
- {
- joint_state_interfaces_.insert(std::make_pair(description.get_name(), description));
- }
- }
-
- /**
- * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo.
- * Separate them into the possible types: Joint and store them.
- */
- virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info)
- {
- auto joint_command_interface_descriptions =
- parse_command_interface_descriptions(hardware_info.joints);
- for (const auto & description : joint_command_interface_descriptions)
- {
- joint_command_interfaces_.insert(std::make_pair(description.get_name(), description));
- }
- }
-
/// Exports all state interfaces for this hardware interface.
/**
* Old way of exporting the StateInterfaces. If a empty vector is returned then
diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp
index 2d0c067606..fc195d0a65 100644
--- a/hardware_interface/include/hardware_interface/component_parser.hpp
+++ b/hardware_interface/include/hardware_interface/component_parser.hpp
@@ -16,6 +16,7 @@
#define HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_
#include
+#include
#include
#include "hardware_interface/hardware_info.hpp"
@@ -41,6 +42,16 @@ HARDWARE_INTERFACE_PUBLIC
std::vector parse_state_interface_descriptions(
const std::vector & component_info);
+/**
+ * \param[in] component_info information about a component (gpio, joint, sensor)
+ * \param[out] state_interfaces_map unordered_map filled with information about hardware's
+ * StateInterfaces for the component which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+void parse_state_interface_descriptions(
+ const std::vector & component_info,
+ std::unordered_map & state_interfaces_map);
+
/**
* \param[in] component_info information about a component (gpio, joint, sensor)
* \return vector filled with information about hardware's CommandInterfaces for the component
@@ -50,5 +61,14 @@ HARDWARE_INTERFACE_PUBLIC
std::vector parse_command_interface_descriptions(
const std::vector & component_info);
+/**
+ * \param[in] component_info information about a component (gpio, joint, sensor)
+ * \param[out] command_interfaces_map unordered_map filled with information about hardware's
+ * CommandInterfaces for the component which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+void parse_command_interface_descriptions(
+ const std::vector & component_info,
+ std::unordered_map & command_interfaces_map);
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_
diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp
index 6fe4f25663..1dfd499c2c 100644
--- a/hardware_interface/include/hardware_interface/handle.hpp
+++ b/hardware_interface/include/hardware_interface/handle.hpp
@@ -17,7 +17,10 @@
#include
#include
+#include
+#include
#include
+#include
#include
#include "hardware_interface/hardware_info.hpp"
@@ -69,13 +72,57 @@ class Handle
{
}
- Handle(const Handle & other) = default;
+ Handle(const Handle & other) noexcept
+ {
+ std::unique_lock lock(other.handle_mutex_);
+ std::unique_lock lock_this(handle_mutex_);
+ prefix_name_ = other.prefix_name_;
+ interface_name_ = other.interface_name_;
+ handle_name_ = other.handle_name_;
+ value_ = other.value_;
+ value_ptr_ = other.value_ptr_;
+ }
- Handle(Handle && other) = default;
+ Handle(Handle && other) noexcept
+ {
+ std::unique_lock lock(other.handle_mutex_);
+ std::unique_lock lock_this(handle_mutex_);
+ prefix_name_ = std::move(other.prefix_name_);
+ interface_name_ = std::move(other.interface_name_);
+ handle_name_ = std::move(other.handle_name_);
+ value_ = std::move(other.value_);
+ value_ptr_ = std::move(other.value_ptr_);
+ }
- Handle & operator=(const Handle & other) = default;
+ Handle & operator=(const Handle & other)
+ {
+ if (this != &other)
+ {
+ std::unique_lock lock(other.handle_mutex_);
+ std::unique_lock lock_this(handle_mutex_);
+ prefix_name_ = other.prefix_name_;
+ interface_name_ = other.interface_name_;
+ handle_name_ = other.handle_name_;
+ value_ = other.value_;
+ value_ptr_ = other.value_ptr_;
+ }
+ return *this;
+ }
- Handle & operator=(Handle && other) = default;
+ Handle & operator=(Handle && other)
+ {
+ if (this != &other)
+ {
+ std::unique_lock lock(other.handle_mutex_);
+ std::unique_lock lock_this(handle_mutex_);
+ prefix_name_ = std::move(other.prefix_name_);
+ interface_name_ = std::move(other.interface_name_);
+ handle_name_ = std::move(other.handle_name_);
+ value_ = std::move(other.value_);
+ value_ptr_ = std::move(other.value_ptr_);
+ }
+ return *this;
+ }
virtual ~Handle() = default;
@@ -95,8 +142,14 @@ class Handle
const std::string & get_prefix_name() const { return prefix_name_; }
+ [[deprecated("Use bool get_value(double & value) instead to retrieve the value.")]]
double get_value() const
{
+ std::shared_lock lock(handle_mutex_, std::try_to_lock);
+ if (!lock.owns_lock())
+ {
+ return std::numeric_limits::quiet_NaN();
+ }
// BEGIN (Handle export change): for backward compatibility
// TODO(Manuel) return value_ if old functionality is removed
THROW_ON_NULLPTR(value_ptr_);
@@ -104,12 +157,33 @@ class Handle
// END
}
- void set_value(double value)
+ [[nodiscard]] bool get_value(double & value) const
+ {
+ std::shared_lock lock(handle_mutex_, std::try_to_lock);
+ if (!lock.owns_lock())
+ {
+ return false;
+ }
+ // BEGIN (Handle export change): for backward compatibility
+ // TODO(Manuel) set value directly if old functionality is removed
+ THROW_ON_NULLPTR(value_ptr_);
+ value = *value_ptr_;
+ return true;
+ // END
+ }
+
+ [[nodiscard]] bool set_value(double value)
{
+ std::unique_lock lock(handle_mutex_, std::try_to_lock);
+ if (!lock.owns_lock())
+ {
+ return false;
+ }
// BEGIN (Handle export change): for backward compatibility
// TODO(Manuel) set value_ directly if old functionality is removed
THROW_ON_NULLPTR(this->value_ptr_);
*this->value_ptr_ = value;
+ return true;
// END
}
@@ -122,6 +196,7 @@ class Handle
// TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed
double * value_ptr_;
// END
+ mutable std::shared_mutex handle_mutex_;
};
class StateInterface : public Handle
diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp
index aa306870a1..6013dea778 100644
--- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp
+++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp
@@ -16,10 +16,13 @@
#define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_
#include
+#include
#include
+#include
#include
#include "hardware_interface/handle.hpp"
+#include "rclcpp/logging.hpp"
namespace hardware_interface
{
@@ -51,6 +54,27 @@ class LoanedCommandInterface
virtual ~LoanedCommandInterface()
{
+ auto logger = rclcpp::get_logger(command_interface_.get_name());
+ RCLCPP_WARN_EXPRESSION(
+ rclcpp::get_logger(get_name()),
+ (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0),
+ "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of "
+ "%u get_value calls",
+ get_name().c_str(), get_value_statistics_.timeout_counter,
+ (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter,
+ get_value_statistics_.failed_counter,
+ (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter,
+ get_value_statistics_.total_counter);
+ RCLCPP_WARN_EXPRESSION(
+ rclcpp::get_logger(get_name()),
+ (set_value_statistics_.failed_counter > 0 || set_value_statistics_.timeout_counter > 0),
+ "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of "
+ "%u set_value calls",
+ get_name().c_str(), set_value_statistics_.timeout_counter,
+ (set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter,
+ set_value_statistics_.failed_counter,
+ (set_value_statistics_.failed_counter * 10.0) / set_value_statistics_.total_counter,
+ set_value_statistics_.total_counter);
if (deleter_)
{
deleter_();
@@ -70,13 +94,70 @@ class LoanedCommandInterface
const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); }
- void set_value(double val) { command_interface_.set_value(val); }
+ template
+ [[nodiscard]] bool set_value(T value, unsigned int max_tries = 10)
+ {
+ unsigned int nr_tries = 0;
+ ++set_value_statistics_.total_counter;
+ while (!command_interface_.set_value(value))
+ {
+ ++set_value_statistics_.failed_counter;
+ ++nr_tries;
+ if (nr_tries == max_tries)
+ {
+ ++set_value_statistics_.timeout_counter;
+ return false;
+ }
+ std::this_thread::yield();
+ }
+ return true;
+ }
- double get_value() const { return command_interface_.get_value(); }
+ double get_value() const
+ {
+ double value;
+ if (get_value(value))
+ {
+ return value;
+ }
+ else
+ {
+ return std::numeric_limits::quiet_NaN();
+ }
+ }
+
+ template
+ [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const
+ {
+ unsigned int nr_tries = 0;
+ ++get_value_statistics_.total_counter;
+ while (!command_interface_.get_value(value))
+ {
+ ++get_value_statistics_.failed_counter;
+ ++nr_tries;
+ if (nr_tries == max_tries)
+ {
+ ++get_value_statistics_.timeout_counter;
+ return false;
+ }
+ std::this_thread::yield();
+ }
+ return true;
+ }
protected:
CommandInterface & command_interface_;
Deleter deleter_;
+
+private:
+ struct HandleRTStatistics
+ {
+ unsigned int total_counter = 0;
+ unsigned int failed_counter = 0;
+ unsigned int timeout_counter = 0;
+ };
+ mutable HandleRTStatistics get_value_statistics_;
+ HandleRTStatistics set_value_statistics_;
};
} // namespace hardware_interface
diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp
index 96cc3e89df..3ebc8c7ca0 100644
--- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp
+++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp
@@ -16,11 +16,13 @@
#define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_
#include
+#include
#include
+#include
#include
#include "hardware_interface/handle.hpp"
-
+#include "rclcpp/logging.hpp"
namespace hardware_interface
{
class LoanedStateInterface
@@ -56,6 +58,17 @@ class LoanedStateInterface
virtual ~LoanedStateInterface()
{
+ auto logger = rclcpp::get_logger(state_interface_.get_name());
+ RCLCPP_WARN_EXPRESSION(
+ logger,
+ (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0),
+ "LoanedStateInterface %s has %u (%.4f %%) timeouts and %u (%.4f %%) missed calls out of %u "
+ "get_value calls",
+ state_interface_.get_name().c_str(), get_value_statistics_.timeout_counter,
+ (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter,
+ get_value_statistics_.failed_counter,
+ (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter,
+ get_value_statistics_.total_counter);
if (deleter_)
{
deleter_();
@@ -75,11 +88,50 @@ class LoanedStateInterface
const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); }
- double get_value() const { return state_interface_.get_value(); }
+ double get_value() const
+ {
+ double value;
+ if (get_value(value))
+ {
+ return value;
+ }
+ else
+ {
+ return std::numeric_limits::quiet_NaN();
+ }
+ }
+
+ template
+ [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const
+ {
+ unsigned int nr_tries = 0;
+ ++get_value_statistics_.total_counter;
+ while (!state_interface_.get_value(value))
+ {
+ ++get_value_statistics_.failed_counter;
+ ++nr_tries;
+ if (nr_tries == max_tries)
+ {
+ ++get_value_statistics_.timeout_counter;
+ return false;
+ }
+ std::this_thread::yield();
+ }
+ return true;
+ }
protected:
const StateInterface & state_interface_;
Deleter deleter_;
+
+private:
+ struct HandleRTStatistics
+ {
+ unsigned int total_counter = 0;
+ unsigned int failed_counter = 0;
+ unsigned int timeout_counter = 0;
+ };
+ mutable HandleRTStatistics get_value_statistics_;
};
} // namespace hardware_interface
diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp
index 50d79d1a45..74c1d04bd7 100644
--- a/hardware_interface/include/hardware_interface/sensor_interface.hpp
+++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp
@@ -123,24 +123,10 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
{
info_ = hardware_info;
- import_state_interface_descriptions(info_);
+ parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_);
return CallbackReturn::SUCCESS;
};
- /**
- * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo.
- * Separate them into the possible types: Sensor and store them.
- */
- virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info)
- {
- auto sensor_state_interface_descriptions =
- parse_state_interface_descriptions(hardware_info.sensors);
- for (const auto & description : sensor_state_interface_descriptions)
- {
- sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description));
- }
- }
-
/// Exports all state interfaces for this hardware interface.
/**
* Old way of exporting the StateInterfaces. If a empty vector is returned then
diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
index 6b0652d3fe..18da0e4012 100644
--- a/hardware_interface/include/hardware_interface/system_interface.hpp
+++ b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -126,57 +126,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
{
info_ = hardware_info;
- import_state_interface_descriptions(info_);
- import_command_interface_descriptions(info_);
+ parse_state_interface_descriptions(info_.joints, joint_state_interfaces_);
+ parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_);
+ parse_state_interface_descriptions(info_.gpios, gpio_state_interfaces_);
+ parse_command_interface_descriptions(info_.joints, joint_command_interfaces_);
+ parse_command_interface_descriptions(info_.gpios, gpio_command_interfaces_);
return CallbackReturn::SUCCESS;
};
- /**
- * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo.
- * Separate them into the possible types: Joint, GPIO, Sensor and store them.
- */
- void import_state_interface_descriptions(const HardwareInfo & hardware_info)
- {
- auto joint_state_interface_descriptions =
- parse_state_interface_descriptions(hardware_info.joints);
- for (const auto & description : joint_state_interface_descriptions)
- {
- joint_state_interfaces_.insert(std::make_pair(description.get_name(), description));
- }
- auto sensor_state_interface_descriptions =
- parse_state_interface_descriptions(hardware_info.sensors);
- for (const auto & description : sensor_state_interface_descriptions)
- {
- sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description));
- }
- auto gpio_state_interface_descriptions =
- parse_state_interface_descriptions(hardware_info.gpios);
- for (const auto & description : gpio_state_interface_descriptions)
- {
- gpio_state_interfaces_.insert(std::make_pair(description.get_name(), description));
- }
- }
-
- /**
- * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo.
- * Separate them into the possible types: Joint and GPIO and store them.
- */
- void import_command_interface_descriptions(const HardwareInfo & hardware_info)
- {
- auto joint_command_interface_descriptions =
- parse_command_interface_descriptions(hardware_info.joints);
- for (const auto & description : joint_command_interface_descriptions)
- {
- joint_command_interfaces_.insert(std::make_pair(description.get_name(), description));
- }
- auto gpio_command_interface_descriptions =
- parse_command_interface_descriptions(hardware_info.gpios);
- for (const auto & description : gpio_command_interface_descriptions)
- {
- gpio_command_interfaces_.insert(std::make_pair(description.get_name(), description));
- }
- }
-
/// Exports all state interfaces for this hardware interface.
/**
* Old way of exporting the StateInterfaces. If a empty vector is returned then
diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml
index 6af9bd002f..dbe9ad5750 100644
--- a/hardware_interface/package.xml
+++ b/hardware_interface/package.xml
@@ -1,7 +1,7 @@
hardware_interface
- 4.18.0
+ 4.20.0
ros2_control hardware interface
Bence Magyar
Denis Štogl
@@ -18,6 +18,7 @@
tinyxml2_vendor
joint_limits
urdf
+ sdformat_urdf
rcutils
rcutils
diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp
index d2ec0f9d53..0f186531e9 100644
--- a/hardware_interface/src/component_parser.cpp
+++ b/hardware_interface/src/component_parser.cpp
@@ -31,6 +31,8 @@
namespace
{
constexpr const auto kRobotTag = "robot";
+constexpr const auto kSDFTag = "sdf";
+constexpr const auto kModelTag = "model";
constexpr const auto kROS2ControlTag = "ros2_control";
constexpr const auto kHardwareTag = "hardware";
constexpr const auto kPluginNameTag = "plugin";
@@ -812,15 +814,26 @@ std::vector parse_control_resources_from_urdf(const std::string &
"invalid URDF passed in to robot parser: " + std::string(doc.ErrorStr()));
}
- // Find robot tag
+ // Find robot or sdf tag
const tinyxml2::XMLElement * robot_it = doc.RootElement();
+ const tinyxml2::XMLElement * ros2_control_it;
- if (std::string(kRobotTag) != robot_it->Name())
+ if (std::string(kRobotTag) == robot_it->Name())
{
- throw std::runtime_error("the robot tag is not root element in URDF");
+ ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag);
+ }
+ else if (std::string(kSDFTag) == robot_it->Name())
+ {
+ // find model tag in sdf tag
+ const tinyxml2::XMLElement * model_it = robot_it->FirstChildElement(kModelTag);
+ ros2_control_it = model_it->FirstChildElement(kROS2ControlTag);
+ }
+ else
+ {
+ throw std::runtime_error(
+ "the robot tag is not root element in URDF or sdf tag is not root element in SDF");
}
- const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag);
if (!ros2_control_it)
{
throw std::runtime_error("no " + std::string(kROS2ControlTag) + " tag");
@@ -929,6 +942,22 @@ std::vector parse_state_interface_descriptions(
return component_state_interface_descriptions;
}
+void parse_state_interface_descriptions(
+ const std::vector & component_info,
+ std::unordered_map & state_interfaces_map)
+{
+ state_interfaces_map.reserve(state_interfaces_map.size() + component_info.size());
+
+ for (const auto & component : component_info)
+ {
+ for (const auto & state_interface : component.state_interfaces)
+ {
+ InterfaceDescription description(component.name, state_interface);
+ state_interfaces_map.insert(std::make_pair(description.get_name(), description));
+ }
+ }
+}
+
std::vector parse_command_interface_descriptions(
const std::vector & component_info)
{
@@ -946,4 +975,20 @@ std::vector parse_command_interface_descriptions(
return component_command_interface_descriptions;
}
+void parse_command_interface_descriptions(
+ const std::vector & component_info,
+ std::unordered_map & command_interfaces_map)
+{
+ command_interfaces_map.reserve(command_interfaces_map.size() + component_info.size());
+
+ for (const auto & component : component_info)
+ {
+ for (const auto & command_interface : component.command_interfaces)
+ {
+ InterfaceDescription description(component.name, command_interface);
+ command_interfaces_map.insert(std::make_pair(description.get_name(), description));
+ }
+ }
+}
+
} // namespace hardware_interface
diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp
index 20e098b62e..3c24b0cc2a 100644
--- a/hardware_interface/test/test_component_parser.cpp
+++ b/hardware_interface/test/test_component_parser.cpp
@@ -1616,3 +1616,38 @@ TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardw
EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "vacuum");
EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum");
}
+
+TEST_F(TestComponentParser, successfully_parse_valid_sdf)
+{
+ std::string sdf_to_test = ros2_control_test_assets::diff_drive_robot_sdf;
+ const auto control_hardware = parse_control_resources_from_urdf(sdf_to_test);
+ ASSERT_THAT(control_hardware, SizeIs(1));
+ const auto hardware_info = control_hardware.front();
+
+ EXPECT_EQ(hardware_info.name, "GazeboSimSystem");
+ EXPECT_EQ(hardware_info.type, "system");
+ ASSERT_THAT(hardware_info.group, IsEmpty());
+ EXPECT_EQ(hardware_info.hardware_plugin_name, "gz_ros2_control/GazeboSimSystem");
+
+ ASSERT_THAT(hardware_info.joints, SizeIs(2));
+
+ EXPECT_EQ(hardware_info.joints[0].name, "left_wheel_joint");
+ EXPECT_EQ(hardware_info.joints[0].type, "joint");
+ ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1));
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-10");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "10");
+ ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(2));
+ EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_VELOCITY);
+ EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, HW_IF_POSITION);
+
+ EXPECT_EQ(hardware_info.joints[1].name, "right_wheel_joint");
+ EXPECT_EQ(hardware_info.joints[1].type, "joint");
+ ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1));
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_VELOCITY);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-10");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "10");
+ ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(2));
+ EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_VELOCITY);
+ EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].name, HW_IF_POSITION);
+}
diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst
index 8a9835038c..8daf6bab93 100644
--- a/hardware_interface_testing/CHANGELOG.rst
+++ b/hardware_interface_testing/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package hardware_interface_testing
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.20.0 (2024-11-08)
+-------------------
+
+4.19.0 (2024-10-26)
+-------------------
+
4.18.0 (2024-10-07)
-------------------
* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_)
diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml
index 7d8ad0a7cd..3966cbc993 100644
--- a/hardware_interface_testing/package.xml
+++ b/hardware_interface_testing/package.xml
@@ -1,7 +1,7 @@
hardware_interface_testing
- 4.18.0
+ 4.20.0
ros2_control hardware interface testing
Bence Magyar
Denis Štogl
diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst
index cc31c4505f..6b00437dd1 100644
--- a/joint_limits/CHANGELOG.rst
+++ b/joint_limits/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package joint_limits
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.20.0 (2024-11-08)
+-------------------
+
+4.19.0 (2024-10-26)
+-------------------
+
4.18.0 (2024-10-07)
-------------------
diff --git a/joint_limits/package.xml b/joint_limits/package.xml
index 5f929868e0..c7fca5f2af 100644
--- a/joint_limits/package.xml
+++ b/joint_limits/package.xml
@@ -1,6 +1,6 @@
joint_limits
- 4.18.0
+ 4.20.0
Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces.
Bence Magyar
diff --git a/ros2_control-not-released.iron.repos b/ros2_control-not-released.iron.repos
deleted file mode 100644
index 56f46b6f79..0000000000
--- a/ros2_control-not-released.iron.repos
+++ /dev/null
@@ -1 +0,0 @@
-repositories:
diff --git a/ros2_control.iron.repos b/ros2_control.iron.repos
deleted file mode 100644
index c93d8f4ef6..0000000000
--- a/ros2_control.iron.repos
+++ /dev/null
@@ -1,9 +0,0 @@
-repositories:
- ros-controls/realtime_tools:
- type: git
- url: https://github.com/ros-controls/realtime_tools.git
- version: master
- ros-controls/control_msgs:
- type: git
- url: https://github.com/ros-controls/control_msgs.git
- version: master
diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst
index 8d78dbdd63..5b57ddc2e2 100644
--- a/ros2_control/CHANGELOG.rst
+++ b/ros2_control/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package ros2_control
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.20.0 (2024-11-08)
+-------------------
+
+4.19.0 (2024-10-26)
+-------------------
+
4.18.0 (2024-10-07)
-------------------
diff --git a/ros2_control/package.xml b/ros2_control/package.xml
index 08f08112df..925ad6e23f 100644
--- a/ros2_control/package.xml
+++ b/ros2_control/package.xml
@@ -1,7 +1,7 @@
ros2_control
- 4.18.0
+ 4.20.0
Metapackage for ROS2 control related packages
Bence Magyar
Denis Štogl
diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst
index a4750ed2e4..cad5325b2e 100644
--- a/ros2_control_test_assets/CHANGELOG.rst
+++ b/ros2_control_test_assets/CHANGELOG.rst
@@ -2,6 +2,14 @@
Changelog for package ros2_control_test_assets
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.20.0 (2024-11-08)
+-------------------
+* Add Support for SDF (`#1763 `_)
+* Contributors: Aarav Gupta
+
+4.19.0 (2024-10-26)
+-------------------
+
4.18.0 (2024-10-07)
-------------------
* Automatic Creation of Handles in HW, Adding Getters/Setters (variant support) (`#1688 `_)
diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp
index cb685823b4..e5a4de8cab 100644
--- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp
+++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp
@@ -1768,6 +1768,225 @@ const auto gripper_hardware_resources_mimic_false_command_if =
)";
+const auto diff_drive_robot_sdf =
+ R"(
+
+
+
+
+
+ true
+
+
+
+ base_link
+ chassis_link
+ 0 0 0.075 0 0 0
+
+
+
+
+
+
+
+ 0.3 0.3 0.15
+
+
+
+
+ 1 1 1 1
+ 1 1 1 1
+
+
+
+
+
+
+ 0.3 0.3 0.15
+
+
+
+
+
+ 0.5
+
+ 0.0046875
+ 0.0
+ 0.0
+ 0.0046875
+ 0.0
+ 0.0075
+
+
+
+
+
+ chassis_link
+ left_wheel_link
+ 0.09 0.16999999999999998 -0.075 -1.5707963267948966 0 0
+
+ 0 0 1
+
+ -inf
+ inf
+
+
+
+
+
+
+
+
+ 0.05
+ 0.04
+
+
+
+ 0 0 1
+ 0 0 1
+
+
+
+
+
+ 0.05
+ 0.04
+
+
+
+
+ 0.1
+
+ 7.583333333333335e-05
+ 0.0
+ 0.0
+ 7.583333333333335e-05
+ 0.0
+ 0.00012500000000000003
+
+
+
+
+
+ chassis_link
+ right_wheel_link
+ 0.09 -0.16999999999999998 -0.075 -1.5707963267948966 0 0
+
+ 0 0 1
+
+
+ -inf
+ inf
+
+
+
+
+
+
+
+
+ 0.05
+ 0.04
+
+
+
+ 0 0 1
+ 0 0 1
+
+
+
+
+
+ 0.05
+ 0.04
+
+
+
+
+ 0.1
+
+ 7.583333333333335e-05
+ 0.0
+ 0.0
+ 7.583333333333335e-05
+ 0.0
+ 0.00012500000000000003
+
+
+
+
+
+ chassis_link
+ caster_link
+ -0.09 0 -0.075 0 0 0
+
+ 1 1 1
+
+ -inf
+ inf
+
+
+
+
+
+
+
+
+ 0.05
+
+
+
+ 0 0 1
+ 0 0 1
+
+
+
+
+
+ 0.05
+
+
+
+
+ 0.1
+
+ 0.00010000000000000002
+ 0.0
+ 0.0
+ 0.00010000000000000002
+ 0.0
+ 0.00010000000000000002
+
+
+
+
+
+ gz_ros2_control/GazeboSimSystem
+
+
+
+ -10
+ 10
+
+
+
+
+
+
+ -10
+ 10
+
+
+
+
+
+
+ /path/to/config.yml
+
+
+
+)";
+
const auto minimal_robot_urdf =
std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail);
const auto minimal_async_robot_urdf =
diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml
index c3f22e7917..bb818063ab 100644
--- a/ros2_control_test_assets/package.xml
+++ b/ros2_control_test_assets/package.xml
@@ -2,7 +2,7 @@
ros2_control_test_assets
- 4.18.0
+ 4.20.0
The package provides shared test resources for ros2_control stack
Bence Magyar
diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst
index f9c320c74f..894bbb8ea3 100644
--- a/ros2controlcli/CHANGELOG.rst
+++ b/ros2controlcli/CHANGELOG.rst
@@ -2,6 +2,14 @@
Changelog for package ros2controlcli
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.20.0 (2024-11-08)
+-------------------
+
+4.19.0 (2024-10-26)
+-------------------
+* [ros2controlcli] Fix the missing exported state interface printing (`#1800 `_)
+* Contributors: Sai Kishor Kothakota
+
4.18.0 (2024-10-07)
-------------------
* [ros2controlcli] add params file parsing to load_controller verb and add namespacing support (`#1703 `_)
diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml
index c2e39ca926..ddb904c432 100644
--- a/ros2controlcli/package.xml
+++ b/ros2controlcli/package.xml
@@ -2,7 +2,7 @@
ros2controlcli
- 4.18.0
+ 4.20.0
The ROS 2 command line tools for ROS2 Control.
diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py
index 2634d726ef..949445ec59 100644
--- a/ros2controlcli/setup.py
+++ b/ros2controlcli/setup.py
@@ -19,7 +19,7 @@
setup(
name=package_name,
- version="4.18.0",
+ version="4.20.0",
packages=find_packages(exclude=["test"]),
data_files=[
("share/" + package_name, ["package.xml"]),
diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst
index 386b3b7f78..1f9ad52f22 100644
--- a/rqt_controller_manager/CHANGELOG.rst
+++ b/rqt_controller_manager/CHANGELOG.rst
@@ -2,6 +2,14 @@
Changelog for package rqt_controller_manager
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.20.0 (2024-11-08)
+-------------------
+
+4.19.0 (2024-10-26)
+-------------------
+* fix: call configure_controller on 'unconfigured' state instead load_controller (`#1794 `_)
+* Contributors: Santosh Govindaraj
+
4.18.0 (2024-10-07)
-------------------
diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml
index ad25593e34..1611ce05a3 100644
--- a/rqt_controller_manager/package.xml
+++ b/rqt_controller_manager/package.xml
@@ -2,7 +2,7 @@
rqt_controller_manager
- 4.18.0
+ 4.20.0
Graphical frontend for interacting with the controller manager.
Bence Magyar
Denis Štogl
diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py
index 4e1bb84627..76d8c706c2 100644
--- a/rqt_controller_manager/setup.py
+++ b/rqt_controller_manager/setup.py
@@ -20,7 +20,7 @@
setup(
name=package_name,
- version="4.18.0",
+ version="4.20.0",
packages=[package_name],
data_files=[
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst
index 1875547497..842cc76488 100644
--- a/transmission_interface/CHANGELOG.rst
+++ b/transmission_interface/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package transmission_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.20.0 (2024-11-08)
+-------------------
+
+4.19.0 (2024-10-26)
+-------------------
+
4.18.0 (2024-10-07)
-------------------
diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml
index 1531e1e173..7ae2f2eda7 100644
--- a/transmission_interface/package.xml
+++ b/transmission_interface/package.xml
@@ -2,7 +2,7 @@
transmission_interface
- 4.18.0
+ 4.20.0
transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this.
Bence Magyar
Denis Štogl