diff --git a/.github/workflows/README.md b/.github/workflows/README.md
index ed64bcd94c..62007ffc2d 100644
--- a/.github/workflows/README.md
+++ b/.github/workflows/README.md
@@ -2,5 +2,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)
[](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-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml
index 5c288fabfb..0e2488d31e 100644
--- a/.github/workflows/humble-abi-compatibility.yml
+++ b/.github/workflows/humble-abi-compatibility.yml
@@ -4,6 +4,15 @@ on:
pull_request:
branches:
- humble
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/humble-abi-compatibility.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control-not-released.humble.repos'
jobs:
abi_check:
diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml
index 2cf14105f5..6392ba8e68 100644
--- a/.github/workflows/humble-binary-build.yml
+++ b/.github/workflows/humble-binary-build.yml
@@ -7,9 +7,27 @@ on:
pull_request:
branches:
- humble
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/humble-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control-not-released.humble.repos'
push:
branches:
- humble
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/humble-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control-not-released.humble.repos'
schedule:
# Run every morning to detect flakiness and broken dependencies
- cron: '03 1 * * *'
diff --git a/.github/workflows/humble-check-docs.yml b/.github/workflows/humble-check-docs.yml
new file mode 100644
index 0000000000..f3c31703cd
--- /dev/null
+++ b/.github/workflows/humble-check-docs.yml
@@ -0,0 +1,18 @@
+name: Humble Check Docs
+
+on:
+ workflow_dispatch:
+ pull_request:
+ branches:
+ - humble
+ paths:
+ - '**.rst'
+ - '**.md'
+ - '**.yaml'
+
+jobs:
+ check-docs:
+ name: Check Docs
+ uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@humble
+ with:
+ ROS2_CONTROL_PR: ${{ github.ref }}
diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml
index 0910572227..209c931d4e 100644
--- a/.github/workflows/humble-coverage-build.yml
+++ b/.github/workflows/humble-coverage-build.yml
@@ -4,9 +4,29 @@ on:
push:
branches:
- humble
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/humble-coverage-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.humble.repos'
+ - 'codecov.yml'
pull_request:
branches:
- humble
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/humble-coverage-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.humble.repos'
+ - 'codecov.yml'
jobs:
coverage_humble:
@@ -14,4 +34,3 @@ jobs:
secrets: inherit
with:
ros_distro: humble
- os_name: ubuntu-22.04
diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml
index 3b8a1c6287..85a1b38241 100644
--- a/.github/workflows/humble-debian-build.yml
+++ b/.github/workflows/humble-debian-build.yml
@@ -4,6 +4,15 @@ on:
pull_request:
branches:
- humble
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/humble-debian-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.humble.repos'
schedule:
# Run every day to detect flakiness and broken dependencies
- cron: '03 1 * * *'
diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml
index be8c84b05b..5bb2408578 100644
--- a/.github/workflows/humble-pre-commit.yml
+++ b/.github/workflows/humble-pre-commit.yml
@@ -11,4 +11,3 @@ jobs:
uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master
with:
ros_distro: humble
- os_name: ubuntu-22.04
diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml
index 4c00d2f2ad..62c5cd62ba 100644
--- a/.github/workflows/humble-rhel-binary-build.yml
+++ b/.github/workflows/humble-rhel-binary-build.yml
@@ -4,6 +4,15 @@ on:
pull_request:
branches:
- humble
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/humble-rhel-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.humble.repos'
schedule:
# Run every day to detect flakiness and broken dependencies
- cron: '03 1 * * *'
diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml
index 19637c4897..560ac37cff 100644
--- a/.github/workflows/humble-semi-binary-build.yml
+++ b/.github/workflows/humble-semi-binary-build.yml
@@ -7,9 +7,27 @@ on:
pull_request:
branches:
- humble
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/humble-semi-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.humble.repos'
push:
branches:
- humble
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/humble-semi-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.humble.repos'
schedule:
# Run every morning to detect flakiness and broken dependencies
- cron: '03 1 * * *'
diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml
index 7b4427d6d6..878ad92677 100644
--- a/.github/workflows/humble-source-build.yml
+++ b/.github/workflows/humble-source-build.yml
@@ -4,6 +4,15 @@ on:
push:
branches:
- humble
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/humble-source-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.humble.repos'
schedule:
# Run every day to detect flakiness and broken dependencies
- cron: '03 3 * * *'
@@ -15,3 +24,4 @@ jobs:
ros_distro: humble
ref: humble
ros2_repo_branch: humble
+ os_name: ubuntu-22.04
diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml
index ab6642625f..c2d9c19110 100644
--- a/.github/workflows/iron-abi-compatibility.yml
+++ b/.github/workflows/iron-abi-compatibility.yml
@@ -4,6 +4,15 @@ on:
pull_request:
branches:
- iron
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/iron-abi-compatibility.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control-not-released.iron.repos'
jobs:
abi_check:
diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml
index 911ccafae5..ef90e256a0 100644
--- a/.github/workflows/iron-binary-build.yml
+++ b/.github/workflows/iron-binary-build.yml
@@ -7,9 +7,27 @@ on:
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 * * *'
diff --git a/.github/workflows/ci-check-docs.yml b/.github/workflows/iron-check-docs.yml
similarity index 57%
rename from .github/workflows/ci-check-docs.yml
rename to .github/workflows/iron-check-docs.yml
index 90a822aa72..e9295dad44 100644
--- a/.github/workflows/ci-check-docs.yml
+++ b/.github/workflows/iron-check-docs.yml
@@ -1,12 +1,18 @@
-name: Check Docs
+name: Iron Check Docs
on:
workflow_dispatch:
pull_request:
+ branches:
+ - iron
+ paths:
+ - '**.rst'
+ - '**.md'
+ - '**.yaml'
jobs:
check-docs:
name: Check Docs
- uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@master
+ uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@iron
with:
ROS2_CONTROL_PR: ${{ github.ref }}
diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml
index d82c52bf51..ff5be81d7d 100644
--- a/.github/workflows/iron-coverage-build.yml
+++ b/.github/workflows/iron-coverage-build.yml
@@ -4,9 +4,29 @@ on:
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:
@@ -14,4 +34,3 @@ jobs:
secrets: inherit
with:
ros_distro: iron
- os_name: ubuntu-22.04
diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml
index ff503d64a9..3cbe0c5127 100644
--- a/.github/workflows/iron-debian-build.yml
+++ b/.github/workflows/iron-debian-build.yml
@@ -4,6 +4,15 @@ on:
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 * * *'
diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml
index 60ad26d073..a128958031 100644
--- a/.github/workflows/iron-pre-commit.yml
+++ b/.github/workflows/iron-pre-commit.yml
@@ -11,4 +11,3 @@ jobs:
uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master
with:
ros_distro: iron
- os_name: ubuntu-22.04
diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml
index 981893524d..f308c495f3 100644
--- a/.github/workflows/iron-rhel-binary-build.yml
+++ b/.github/workflows/iron-rhel-binary-build.yml
@@ -4,6 +4,15 @@ on:
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 * * *'
diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml
index 59f8f347dd..30a83e5367 100644
--- a/.github/workflows/iron-semi-binary-build.yml
+++ b/.github/workflows/iron-semi-binary-build.yml
@@ -7,9 +7,27 @@ on:
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 * * *'
diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml
index 3609dcfc41..3b7c53f6ff 100644
--- a/.github/workflows/iron-source-build.yml
+++ b/.github/workflows/iron-source-build.yml
@@ -4,6 +4,15 @@ on:
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 * * *'
@@ -15,3 +24,4 @@ jobs:
ros_distro: iron
ref: iron
ros2_repo_branch: iron
+ os_name: ubuntu-22.04
diff --git a/.github/workflows/jazzy-abi-compatibility.yml b/.github/workflows/jazzy-abi-compatibility.yml
new file mode 100644
index 0000000000..367b3736fb
--- /dev/null
+++ b/.github/workflows/jazzy-abi-compatibility.yml
@@ -0,0 +1,27 @@
+name: Jazzy - ABI Compatibility Check
+on:
+ workflow_dispatch:
+ pull_request:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/jazzy-abi-compatibility.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control-not-released.jazzy.repos'
+
+jobs:
+ abi_check:
+ runs-on: ubuntu-latest
+ steps:
+ - uses: actions/checkout@v4
+ - uses: ros-industrial/industrial_ci@master
+ env:
+ ROS_DISTRO: jazzy
+ ROS_REPO: testing
+ ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }}
+ NOT_TEST_BUILD: true
diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml
new file mode 100644
index 0000000000..5be853ebfc
--- /dev/null
+++ b/.github/workflows/jazzy-binary-build.yml
@@ -0,0 +1,47 @@
+name: Jazzy Binary Build
+# author: Denis Štogl
+# description: 'Build & test all dependencies from released (binary) packages.'
+
+on:
+ workflow_dispatch:
+ pull_request:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/jazzy-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control-not-released.jazzy.repos'
+ push:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/jazzy-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control-not-released.jazzy.repos'
+ schedule:
+ # Run every morning to detect flakiness and broken dependencies
+ - cron: '03 1 * * *'
+
+jobs:
+ binary:
+ uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+ strategy:
+ fail-fast: false
+ matrix:
+ ROS_DISTRO: [jazzy]
+ ROS_REPO: [main, testing]
+ with:
+ ros_distro: ${{ matrix.ROS_DISTRO }}
+ ros_repo: ${{ matrix.ROS_REPO }}
+ upstream_workspace: ros2_control-not-released.${{ matrix.ROS_DISTRO }}.repos
+ ref_for_scheduled_build: master
diff --git a/.github/workflows/jazzy-check-docs.yml b/.github/workflows/jazzy-check-docs.yml
new file mode 100644
index 0000000000..cbdf6c30bd
--- /dev/null
+++ b/.github/workflows/jazzy-check-docs.yml
@@ -0,0 +1,18 @@
+name: Jazzy Check Docs
+
+on:
+ workflow_dispatch:
+ pull_request:
+ branches:
+ - master
+ paths:
+ - '**.rst'
+ - '**.md'
+ - '**.yaml'
+
+jobs:
+ check-docs:
+ name: Check Docs
+ uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@jazzy
+ with:
+ ROS2_CONTROL_PR: ${{ github.ref }}
diff --git a/.github/workflows/jazzy-coverage-build.yml b/.github/workflows/jazzy-coverage-build.yml
new file mode 100644
index 0000000000..aa345d1e80
--- /dev/null
+++ b/.github/workflows/jazzy-coverage-build.yml
@@ -0,0 +1,35 @@
+name: Coverage Build - Jazzy
+on:
+ workflow_dispatch:
+ # TODO(anyone) activate when branched for Jazzy
+ # push:
+ # branches:
+ # - master
+ # paths:
+ # - '**.hpp'
+ # - '**.h'
+ # - '**.cpp'
+ # - '.github/workflows/jazzy-coverage-build.yml'
+ # - '**/package.xml'
+ # - '**/CMakeLists.txt'
+ # - 'ros2_control.jazzy.repos'
+ # - 'codecov.yml'
+ # pull_request:
+ # branches:
+ # - master
+ # paths:
+ # - '**.hpp'
+ # - '**.h'
+ # - '**.cpp'
+ # - '.github/workflows/jazzy-coverage-build.yml'
+ # - '**/package.xml'
+ # - '**/CMakeLists.txt'
+ # - 'ros2_control.jazzy.repos'
+ # - 'codecov.yml'
+
+jobs:
+ coverage_jazzy:
+ uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master
+ secrets: inherit
+ with:
+ ros_distro: jazzy
diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml
new file mode 100644
index 0000000000..4ec6a29fff
--- /dev/null
+++ b/.github/workflows/jazzy-debian-build.yml
@@ -0,0 +1,32 @@
+name: Debian Jazzy Source Build
+on:
+ workflow_dispatch:
+ pull_request:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/jazzy-debian-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.jazzy.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: [jazzy]
+ with:
+ ros_distro: ${{ matrix.ROS_DISTRO }}
+ upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos
+ ref_for_scheduled_build: master
+ skip_packages: rqt_controller_manager
diff --git a/.github/workflows/jazzy-pre-commit.yml b/.github/workflows/jazzy-pre-commit.yml
new file mode 100644
index 0000000000..d9ec610bbc
--- /dev/null
+++ b/.github/workflows/jazzy-pre-commit.yml
@@ -0,0 +1,13 @@
+name: Pre-Commit - Jazzy
+
+on:
+ workflow_dispatch:
+ pull_request:
+ branches:
+ - master
+
+jobs:
+ pre-commit:
+ uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master
+ with:
+ ros_distro: jazzy
diff --git a/.github/workflows/jazzy-rhel-binary-build.yml b/.github/workflows/jazzy-rhel-binary-build.yml
new file mode 100644
index 0000000000..0dcc912dab
--- /dev/null
+++ b/.github/workflows/jazzy-rhel-binary-build.yml
@@ -0,0 +1,31 @@
+name: RHEL Jazzy Semi-Binary Build
+on:
+ workflow_dispatch:
+ pull_request:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/jazzy-rhel-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.jazzy.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: [jazzy]
+ with:
+ ros_distro: ${{ matrix.ROS_DISTRO }}
+ upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos
+ ref_for_scheduled_build: master
+ skip_packages: rqt_controller_manager
diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml
new file mode 100644
index 0000000000..9634732cf9
--- /dev/null
+++ b/.github/workflows/jazzy-semi-binary-build.yml
@@ -0,0 +1,47 @@
+name: Jazzy Semi-Binary Build
+# author: Denis Štogl
+# description: 'Build & test all dependencies from released (binary) packages.'
+
+on:
+ workflow_dispatch:
+ pull_request:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/jazzy-semi-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.jazzy.repos'
+ push:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/jazzy-semi-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.jazzy.repos'
+ schedule:
+ # Run every morning to detect flakiness and broken dependencies
+ - cron: '03 1 * * *'
+
+jobs:
+ binary:
+ uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+ strategy:
+ fail-fast: false
+ matrix:
+ ROS_DISTRO: [jazzy]
+ ROS_REPO: [main, testing]
+ with:
+ ros_distro: ${{ matrix.ROS_DISTRO }}
+ ros_repo: ${{ matrix.ROS_REPO }}
+ upstream_workspace: ros2_control.${{ matrix.ROS_DISTRO }}.repos
+ ref_for_scheduled_build: master
diff --git a/.github/workflows/jazzy-source-build.yml b/.github/workflows/jazzy-source-build.yml
new file mode 100644
index 0000000000..65066a4bf2
--- /dev/null
+++ b/.github/workflows/jazzy-source-build.yml
@@ -0,0 +1,27 @@
+name: Jazzy Source Build
+on:
+ workflow_dispatch:
+ push:
+ branches:
+ - master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/jazzy-source-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.jazzy.repos'
+ schedule:
+ # Run every day to detect flakiness and broken dependencies
+ - cron: '03 3 * * *'
+
+jobs:
+ source:
+ uses: ros-controls/ros2_control_ci/.github/workflows/reusable-ros-tooling-source-build.yml@master
+ with:
+ ros_distro: jazzy
+ ref: master
+ ros2_repo_branch: master
+ container: ubuntu:24.04
diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml
index 73055ef3e5..b7828390fb 100644
--- a/.github/workflows/rolling-abi-compatibility.yml
+++ b/.github/workflows/rolling-abi-compatibility.yml
@@ -4,6 +4,15 @@ on:
pull_request:
branches:
- master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-abi-compatibility.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control-not-released.rolling.repos'
jobs:
abi_check:
diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml
index b083cc46fc..24a28f16ae 100644
--- a/.github/workflows/rolling-binary-build.yml
+++ b/.github/workflows/rolling-binary-build.yml
@@ -7,9 +7,27 @@ on:
pull_request:
branches:
- master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control-not-released.rolling.repos'
push:
branches:
- master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control-not-released.rolling.repos'
schedule:
# Run every morning to detect flakiness and broken dependencies
- cron: '03 1 * * *'
diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml
new file mode 100644
index 0000000000..bd83c0caca
--- /dev/null
+++ b/.github/workflows/rolling-check-docs.yml
@@ -0,0 +1,19 @@
+name: Rolling Check Docs
+
+on:
+ workflow_dispatch:
+ pull_request:
+ branches:
+ - master
+ paths:
+ - '**.rst'
+ - '**.md'
+ - '**.yaml'
+ - '.github/workflows/rolling-check-docs.yml'
+
+jobs:
+ check-docs:
+ name: Check Docs
+ uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@rolling
+ with:
+ ROS2_CONTROL_PR: ${{ github.ref }}
diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml
index 4d4750c54c..45b10876e7 100644
--- a/.github/workflows/rolling-coverage-build.yml
+++ b/.github/workflows/rolling-coverage-build.yml
@@ -4,9 +4,29 @@ on:
push:
branches:
- master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-coverage-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.rolling.repos'
+ - 'codecov.yml'
pull_request:
branches:
- master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-coverage-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.rolling.repos'
+ - 'codecov.yml'
jobs:
coverage_rolling:
@@ -14,4 +34,3 @@ jobs:
secrets: inherit
with:
ros_distro: rolling
- os_name: ubuntu-22.04
diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml
index e792963cc6..00d4ad844b 100644
--- a/.github/workflows/rolling-debian-build.yml
+++ b/.github/workflows/rolling-debian-build.yml
@@ -4,6 +4,15 @@ on:
pull_request:
branches:
- master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-debian-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.rolling.repos'
schedule:
# Run every day to detect flakiness and broken dependencies
- cron: '03 1 * * *'
diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml
index 9c87311bd7..7bc07ba802 100644
--- a/.github/workflows/rolling-pre-commit.yml
+++ b/.github/workflows/rolling-pre-commit.yml
@@ -11,4 +11,3 @@ jobs:
uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master
with:
ros_distro: rolling
- os_name: ubuntu-22.04
diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml
index 38f375d6b6..c8939d6015 100644
--- a/.github/workflows/rolling-rhel-binary-build.yml
+++ b/.github/workflows/rolling-rhel-binary-build.yml
@@ -4,6 +4,15 @@ on:
pull_request:
branches:
- master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-rhel-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.jazzy.repos'
schedule:
# Run every day to detect flakiness and broken dependencies
- cron: '03 1 * * *'
diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml
index f5c7885139..4cdb7ab585 100644
--- a/.github/workflows/rolling-semi-binary-build.yml
+++ b/.github/workflows/rolling-semi-binary-build.yml
@@ -7,9 +7,27 @@ on:
pull_request:
branches:
- master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-semi-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.rolling.repos'
push:
branches:
- master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-semi-binary-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.rolling.repos'
schedule:
# Run every morning to detect flakiness and broken dependencies
- cron: '03 1 * * *'
diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml
index f34a8e6bb5..9bbf09cda4 100644
--- a/.github/workflows/rolling-source-build.yml
+++ b/.github/workflows/rolling-source-build.yml
@@ -4,6 +4,15 @@ on:
push:
branches:
- master
+ paths:
+ - '**.hpp'
+ - '**.h'
+ - '**.cpp'
+ - '**.py'
+ - '.github/workflows/rolling-source-build.yml'
+ - '**/package.xml'
+ - '**/CMakeLists.txt'
+ - 'ros2_control.rolling.repos'
schedule:
# Run every day to detect flakiness and broken dependencies
- cron: '03 3 * * *'
@@ -15,3 +24,4 @@ jobs:
ros_distro: rolling
ref: master
ros2_repo_branch: master
+ container: ubuntu:24.04
diff --git a/.github/workflows/update-pre-commit.yml b/.github/workflows/update-pre-commit.yml
index 8b9545dff1..6bedaa0c97 100644
--- a/.github/workflows/update-pre-commit.yml
+++ b/.github/workflows/update-pre-commit.yml
@@ -5,7 +5,7 @@ name: Auto Update pre-commit
on:
workflow_dispatch:
schedule:
- - cron: '0 0 * * 0' # Run every Sunday at midnight
+ - cron: '0 0 1 * *' # Runs at 00:00, on day 1 of the month
jobs:
auto_update_and_create_pr:
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 63dc2afe69..8e604b2f7f 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -49,7 +49,7 @@ repos:
args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
- repo: https://github.com/psf/black
- rev: 24.3.0
+ rev: 24.4.2
hooks:
- id: black
args: ["--line-length=99"]
@@ -62,7 +62,7 @@ repos:
# CPP hooks
- repo: https://github.com/pre-commit/mirrors-clang-format
- rev: v18.1.3
+ rev: v18.1.5
hooks:
- id: clang-format
args: ['-fallback-style=none', '-i']
@@ -104,6 +104,7 @@ repos:
description: Check if copyright notice is available in all files.
entry: ament_copyright
language: system
+ exclude: .*/conf\.py$
# Docs - RestructuredText hooks
- repo: https://github.com/PyCQA/doc8
@@ -124,14 +125,14 @@ repos:
# Spellcheck in comments and docs
# skipping of *.svg files is not working...
- repo: https://github.com/codespell-project/codespell
- rev: v2.2.6
+ rev: v2.3.0
hooks:
- id: codespell
args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel']
exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
- repo: https://github.com/python-jsonschema/check-jsonschema
- rev: 0.28.1
+ rev: 0.28.4
hooks:
- id: check-github-workflows
args: ["--verbose"]
diff --git a/README.md b/README.md
index e88ca5b7bd..40d2a3c189 100644
--- a/README.md
+++ b/README.md
@@ -12,6 +12,7 @@ For more, please check the [documentation](https://control.ros.org/).
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)
diff --git a/codecov.yml b/codecov.yml
index d8a5fde3e0..97106f32ff 100644
--- a/codecov.yml
+++ b/codecov.yml
@@ -22,4 +22,5 @@ flags:
- controller_interface
- controller_manager
- hardware_interface
+ - joint_limits
- transmission_interface
diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst
index 3ed820dfe5..8d77f4203f 100644
--- a/controller_interface/CHANGELOG.rst
+++ b/controller_interface/CHANGELOG.rst
@@ -2,6 +2,22 @@
Changelog for package controller_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.11.0 (2024-05-14)
+-------------------
+* Fix dependencies for source build (`#1533 `_)
+* Add find_package for ament_cmake_gen_version_h (`#1534 `_)
+* Contributors: Christoph Fröhlich
+
+4.10.0 (2024-05-08)
+-------------------
+* Working async controllers and components [not synchronized] (`#1041 `_)
+* Contributors: Márk Szitanics
+
+4.9.0 (2024-04-30)
+------------------
+* return the proper const object of the pointer in the const method (`#1494 `_)
+* Contributors: Sai Kishor Kothakota
+
4.8.0 (2024-03-27)
------------------
* generate version.h file per package using the ament_generate_version_header (`#1449 `_)
diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt
index 2b7ccb9203..3dc3bc4d0a 100644
--- a/controller_interface/CMakeLists.txt
+++ b/controller_interface/CMakeLists.txt
@@ -12,6 +12,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
)
find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_gen_version_h REQUIRED)
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach()
diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp
new file mode 100644
index 0000000000..5601ff4703
--- /dev/null
+++ b/controller_interface/include/controller_interface/async_controller.hpp
@@ -0,0 +1,111 @@
+// 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_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 3f75d7e3c0..888d499688 100644
--- a/controller_interface/include/controller_interface/controller_interface_base.hpp
+++ b/controller_interface/include/controller_interface/controller_interface_base.hpp
@@ -145,7 +145,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
std::shared_ptr get_node();
CONTROLLER_INTERFACE_PUBLIC
- std::shared_ptr get_node() const;
+ std::shared_ptr get_node() const;
CONTROLLER_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & get_state() const;
diff --git a/controller_interface/package.xml b/controller_interface/package.xml
index 8143d8f9c5..9a99449043 100644
--- a/controller_interface/package.xml
+++ b/controller_interface/package.xml
@@ -2,7 +2,7 @@
controller_interface
- 4.8.0
+ 4.11.0
Description of controller_interface
Bence Magyar
Denis Štogl
diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
index e48e1d21b3..6b87ba5cda 100644
--- a/controller_interface/src/controller_interface_base.cpp
+++ b/controller_interface/src/controller_interface_base.cpp
@@ -121,7 +121,7 @@ std::shared_ptr ControllerInterfaceBase::get_no
return node_;
}
-std::shared_ptr ControllerInterfaceBase::get_node() const
+std::shared_ptr ControllerInterfaceBase::get_node() const
{
if (!node_.get())
{
diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp
index 3976b2a81e..03838b1a2e 100644
--- a/controller_interface/test/test_controller_interface.cpp
+++ b/controller_interface/test/test_controller_interface.cpp
@@ -88,7 +88,8 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
ASSERT_EQ(controller.get_update_rate(), 2812u);
// Test updating of update_rate parameter
- EXPECT_EQ(std::system("ros2 param set /testable_controller_interface update_rate 623"), 0);
+ auto res = controller.get_node()->set_parameter(rclcpp::Parameter("update_rate", 623));
+ EXPECT_EQ(res.successful, true);
// Keep the same update rate until transition from 'UNCONFIGURED' TO 'INACTIVE' does not happen
controller.configure(); // No transition so the update rate should stay intact
ASSERT_NE(controller.get_update_rate(), 623u);
diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst
index 53005b3800..2248f5f668 100644
--- a/controller_manager/CHANGELOG.rst
+++ b/controller_manager/CHANGELOG.rst
@@ -2,6 +2,26 @@
Changelog for package controller_manager
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.11.0 (2024-05-14)
+-------------------
+* Add find_package for ament_cmake_gen_version_h (`#1534 `_)
+* Contributors: Christoph Fröhlich
+
+4.10.0 (2024-05-08)
+-------------------
+* allow extra spawner arguments to not declare every argument in launch utils (`#1505 `_)
+* Working async controllers and components [not synchronized] (`#1041 `_)
+* Add fallback controllers list to the ControllerInfo (`#1503 `_)
+* Add a functionality to look for the controller type in the params file when not parsed (`#1502 `_)
+* Add controller exception handling in controller manager (`#1507 `_)
+* Contributors: Márk Szitanics, Sai Kishor Kothakota
+
+4.9.0 (2024-04-30)
+------------------
+* Deactivate the controllers when they return error similar to hardware (`#1499 `_)
+* Component parser: Get mimic information from URDF (`#1256 `_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota
+
4.8.0 (2024-03-27)
------------------
* generate version.h file per package using the ament_generate_version_header (`#1449 `_)
diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
index e267856eb1..c3c0de7739 100644
--- a/controller_manager/CMakeLists.txt
+++ b/controller_manager/CMakeLists.txt
@@ -21,6 +21,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(ament_cmake_core REQUIRED)
+find_package(ament_cmake_gen_version_h REQUIRED)
find_package(backward_ros REQUIRED)
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
@@ -190,6 +191,12 @@ if(BUILD_TESTING)
ros2_control_test_assets::ros2_control_test_assets
)
+ install(FILES test/test_controller_spawner_with_fallback_controllers.yaml
+ DESTINATION test)
+
+ install(FILES test/test_controller_spawner_with_type.yaml
+ DESTINATION test)
+
ament_add_gmock(test_hardware_management_srvs
test/test_hardware_management_srvs.cpp
)
diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py
index 5a23c02cec..01528552d0 100644
--- a/controller_manager/controller_manager/launch_utils.py
+++ b/controller_manager/controller_manager/launch_utils.py
@@ -12,6 +12,7 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+import warnings
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PythonExpression
@@ -20,7 +21,7 @@
def generate_load_controller_launch_description(
- controller_name, controller_type=None, controller_params_file=None
+ controller_name, controller_type=None, controller_params_file=None, extra_spawner_args=[]
):
"""
Generate launch description for loading a controller using spawner.
@@ -39,7 +40,8 @@ def generate_load_controller_launch_description(
'joint_state_broadcaster',
controller_type='joint_state_broadcaster/JointStateBroadcaster',
controller_params_file=os.path.join(get_package_share_directory('my_pkg'),
- 'config', 'controller_params.yaml')
+ 'config', 'controller_params.yaml'),
+ extra_spawner_args=[--load-only]
)
"""
@@ -61,6 +63,12 @@ def generate_load_controller_launch_description(
]
if controller_type:
+ warnings.warn(
+ "The 'controller_type' argument is deprecated and will be removed in future releases."
+ " Declare the controller type parameter in the param file instead.",
+ DeprecationWarning,
+ stacklevel=2,
+ )
spawner_arguments += ["--controller-type", controller_type]
if controller_params_file:
@@ -79,6 +87,9 @@ def generate_load_controller_launch_description(
)
]
+ if extra_spawner_args:
+ spawner_arguments += extra_spawner_args
+
spawner = Node(
package="controller_manager",
executable="spawner",
diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py
index 1d11d80fe1..dbefd360b9 100644
--- a/controller_manager/controller_manager/spawner.py
+++ b/controller_manager/controller_manager/spawner.py
@@ -19,6 +19,7 @@
import sys
import time
import warnings
+import yaml
from controller_manager import (
configure_controller,
@@ -135,6 +136,21 @@ def is_controller_loaded(node, controller_manager, controller_name):
return any(c.name == controller_name for c in controllers)
+def get_parameter_from_param_file(controller_name, parameter_file, parameter_name):
+ with open(parameter_file) as f:
+ parameters = yaml.safe_load(f)
+ if controller_name in parameters:
+ value = parameters[controller_name]
+ if not isinstance(value, dict) or "ros__parameters" not in value:
+ raise RuntimeError(
+ f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {controller_name}"
+ )
+ if parameter_name in parameters[controller_name]["ros__parameters"]:
+ return parameters[controller_name]["ros__parameters"][parameter_name]
+ else:
+ return None
+
+
def main(args=None):
rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
parser = argparse.ArgumentParser()
@@ -170,7 +186,7 @@ def main(args=None):
parser.add_argument(
"-t",
"--controller-type",
- help="If not provided it should exist in the controller manager namespace",
+ help="If not provided it should exist in the controller manager namespace (deprecated)",
default=None,
required=False,
)
@@ -194,6 +210,14 @@ def main(args=None):
action="store_true",
required=False,
)
+ parser.add_argument(
+ "--fallback_controllers",
+ help="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",
+ default=None,
+ nargs="+",
+ )
command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
args = parser.parse_args(command_line_args)
@@ -201,9 +225,16 @@ def main(args=None):
controller_manager_name = args.controller_manager
controller_namespace = args.namespace
param_file = args.param_file
- controller_type = args.controller_type
controller_manager_timeout = args.controller_manager_timeout
+ if args.controller_type:
+ warnings.filterwarnings("always")
+ warnings.warn(
+ "The '--controller-type' argument is deprecated and will be removed in future releases."
+ " Declare the controller type parameter in the param file instead.",
+ DeprecationWarning,
+ )
+
if param_file and not os.path.isfile(param_file):
raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
@@ -226,6 +257,8 @@ def main(args=None):
return 1
for controller_name in controller_names:
+ fallback_controllers = args.fallback_controllers
+ controller_type = args.controller_type
prefixed_controller_name = controller_name
if controller_namespace:
prefixed_controller_name = controller_namespace + "/" + controller_name
@@ -237,6 +270,10 @@ def main(args=None):
+ bcolors.ENDC
)
else:
+ if not controller_type and param_file:
+ controller_type = get_parameter_from_param_file(
+ controller_name, param_file, "type"
+ )
if controller_type:
parameter = Parameter()
parameter.name = prefixed_controller_name + ".type"
@@ -301,6 +338,43 @@ def main(args=None):
)
return 1
+ if not fallback_controllers and param_file:
+ fallback_controllers = get_parameter_from_param_file(
+ controller_name, param_file, "fallback_controllers"
+ )
+
+ if fallback_controllers:
+ parameter = Parameter()
+ parameter.name = prefixed_controller_name + ".fallback_controllers"
+ parameter.value = get_parameter_value(string_value=str(fallback_controllers))
+
+ response = call_set_parameters(
+ node=node, node_name=controller_manager_name, parameters=[parameter]
+ )
+ assert len(response.results) == 1
+ result = response.results[0]
+ if result.successful:
+ node.get_logger().info(
+ bcolors.OKCYAN
+ + 'Setting fallback_controllers to ["'
+ + ",".join(fallback_controllers)
+ + '"] for '
+ + bcolors.BOLD
+ + prefixed_controller_name
+ + bcolors.ENDC
+ )
+ else:
+ node.get_logger().fatal(
+ bcolors.FAIL
+ + 'Could not set fallback_controllers to ["'
+ + ",".join(fallback_controllers)
+ + '"] for '
+ + bcolors.BOLD
+ + prefixed_controller_name
+ + bcolors.ENDC
+ )
+ return 1
+
ret = load_controller(node, controller_manager_name, controller_name)
if not ret.ok:
node.get_logger().fatal(
diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst
index c8aa68d774..4deb85291b 100644
--- a/controller_manager/doc/userdoc.rst
+++ b/controller_manager/doc/userdoc.rst
@@ -225,6 +225,10 @@ Note that not all controllers have to be restarted, e.g., broadcasters.
Restarting hardware
^^^^^^^^^^^^^^^^^^^^^
-If hardware gets restarted then you should go through its lifecycle again.
-This can be simply achieved by returning ``ERROR`` from ``write`` and ``read`` methods of interface implementation.
-**NOT IMPLEMENTED YET - PLEASE STOP/RESTART ALL CONTROLLERS MANUALLY FOR NOW** The controller manager detects that and stops all the controllers that are commanding that hardware and restarts broadcasters that are listening to its states.
+If hardware gets restarted then you should go through its lifecycle again in order to reconfigure and export the interfaces
+
+Hardware and Controller Errors
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+If the hardware during it's ``read`` or ``write`` method returns ``return_type::ERROR``, the controller manager will stop all controllers that are using the hardware's command and state interfaces.
+Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller. In future, the controller manager will try to start any fallback controllers if available.
diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
index 9a709c5f8f..845fa2dee0 100644
--- a/controller_manager/include/controller_manager/controller_manager.hpp
+++ b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -23,6 +23,7 @@
#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"
@@ -196,6 +197,15 @@ class ControllerManager : public rclcpp::Node
CONTROLLER_MANAGER_PUBLIC
unsigned int get_update_rate() const;
+ /// Deletes all async controllers and components.
+ /**
+ * Needed to join the threads immediately after the control loop is ended
+ * to avoid unnecessary iterations. Otherwise
+ * the threads will be joined only when the controller manager gets destroyed.
+ */
+ CONTROLLER_MANAGER_PUBLIC
+ void shutdown_async_controllers_and_components();
+
protected:
CONTROLLER_MANAGER_PUBLIC
void init_services();
@@ -563,65 +573,7 @@ class ControllerManager : public rclcpp::Node
SwitchParams switch_params_;
- class ControllerThreadWrapper
- {
- public:
- ControllerThreadWrapper(
- std::shared_ptr & controller,
- int cm_update_rate)
- : terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate)
- {
- }
-
- ControllerThreadWrapper(const ControllerThreadWrapper & t) = delete;
- ControllerThreadWrapper(ControllerThreadWrapper && t) = default;
- ~ControllerThreadWrapper()
- {
- terminated_.store(true, std::memory_order_seq_cst);
- if (thread_.joinable())
- {
- thread_.join();
- }
- }
-
- void activate()
- {
- thread_ = std::thread(&ControllerThreadWrapper::call_controller_update, this);
- }
-
- void call_controller_update()
- {
- 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(); // determines if the controller's or CM's update rate is used
-
- 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_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
- {
- // critical section, not implemented yet
- }
-
- 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_;
- };
-
- std::unordered_map>
+ std::unordered_map>
async_controller_threads_;
};
diff --git a/controller_manager/package.xml b/controller_manager/package.xml
index 7de94fc053..e2aa998332 100644
--- a/controller_manager/package.xml
+++ b/controller_manager/package.xml
@@ -2,7 +2,7 @@
controller_manager
- 4.8.0
+ 4.11.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 c48b0e53ac..9809c48cb2 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -482,6 +482,17 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
controller_spec.info.parameters_file = parameters_file;
}
+ const std::string fallback_ctrl_param = controller_name + ".fallback_controllers";
+ std::vector fallback_controllers;
+ if (!has_parameter(fallback_ctrl_param))
+ {
+ declare_parameter(fallback_ctrl_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY);
+ }
+ if (get_parameter(fallback_ctrl_param, fallback_controllers) && !fallback_controllers.empty())
+ {
+ controller_spec.info.fallback_controllers_names = fallback_controllers;
+ }
+
return add_controller_impl(controller_spec);
}
@@ -562,11 +573,26 @@ controller_interface::return_type ControllerManager::unload_controller(
get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str());
// TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
// cleaning-up controllers?
- const auto new_state = controller.c->get_node()->cleanup();
- if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+ try
{
- RCLCPP_WARN(
- get_logger(), "Failed to clean-up the controller '%s' before unloading!",
+ const auto new_state = controller.c->get_node()->cleanup();
+ if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+ {
+ RCLCPP_WARN(
+ get_logger(), "Failed to clean-up the controller '%s' before unloading!",
+ controller_name.c_str());
+ }
+ }
+ catch (const std::exception & e)
+ {
+ RCLCPP_ERROR(
+ get_logger(), "Failed to clean-up the controller '%s' before unloading: %s",
+ controller_name.c_str(), e.what());
+ }
+ catch (...)
+ {
+ RCLCPP_ERROR(
+ get_logger(), "Failed to clean-up the controller '%s' before unloading",
controller_name.c_str());
}
}
@@ -631,22 +657,49 @@ controller_interface::return_type ControllerManager::configure_controller(
get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str());
// TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
// cleaning-up controllers?
- new_state = controller->get_node()->cleanup();
- if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+ try
+ {
+ new_state = controller->get_node()->cleanup();
+ if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+ {
+ RCLCPP_ERROR(
+ get_logger(), "Controller '%s' can not be cleaned-up before configuring",
+ controller_name.c_str());
+ return controller_interface::return_type::ERROR;
+ }
+ }
+ catch (...)
{
RCLCPP_ERROR(
- get_logger(), "Controller '%s' can not be cleaned-up before configuring",
+ get_logger(), "Caught exception while cleaning-up controller '%s' before configuring",
controller_name.c_str());
return controller_interface::return_type::ERROR;
}
}
- new_state = controller->configure();
- if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+ try
+ {
+ new_state = controller->configure();
+ if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+ {
+ RCLCPP_ERROR(
+ get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.",
+ controller_name.c_str(), new_state.label().c_str());
+ return controller_interface::return_type::ERROR;
+ }
+ }
+ catch (const std::exception & e)
{
RCLCPP_ERROR(
- get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.",
- controller_name.c_str(), new_state.label().c_str());
+ get_logger(), "Caught exception while configuring controller '%s': %s",
+ controller_name.c_str(), e.what());
+ return controller_interface::return_type::ERROR;
+ }
+ catch (...)
+ {
+ RCLCPP_ERROR(
+ get_logger(), "Caught unknown exception while configuring controller '%s'",
+ controller_name.c_str());
return controller_interface::return_type::ERROR;
}
@@ -654,7 +707,8 @@ controller_interface::return_type ControllerManager::configure_controller(
if (controller->is_async())
{
async_controller_threads_.emplace(
- controller_name, std::make_unique(controller, update_rate_));
+ controller_name,
+ std::make_unique(controller, update_rate_));
}
const auto controller_update_rate = controller->get_update_rate();
@@ -1300,14 +1354,35 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
}
const rclcpp::NodeOptions controller_node_options = determine_controller_node_options(controller);
- if (
- controller.c->init(
- controller.info.name, robot_description_, get_update_rate(), get_namespace(),
- controller_node_options) == controller_interface::return_type::ERROR)
+ // Catch whatever exception the controller might throw
+ try
+ {
+ if (
+ controller.c->init(
+ controller.info.name, robot_description_, get_update_rate(), get_namespace(),
+ controller_node_options) == controller_interface::return_type::ERROR)
+ {
+ to.clear();
+ RCLCPP_ERROR(
+ get_logger(), "Could not initialize the controller named '%s'",
+ controller.info.name.c_str());
+ return nullptr;
+ }
+ }
+ catch (const std::exception & e)
{
to.clear();
RCLCPP_ERROR(
- get_logger(), "Could not initialize the controller named '%s'", controller.info.name.c_str());
+ get_logger(), "Caught exception while initializing controller '%s': %s",
+ controller.info.name.c_str(), e.what());
+ return nullptr;
+ }
+ catch (...)
+ {
+ to.clear();
+ RCLCPP_ERROR(
+ get_logger(), "Caught unknown exception while initializing controller '%s'",
+ controller.info.name.c_str());
return nullptr;
}
@@ -1350,18 +1425,37 @@ void ControllerManager::deactivate_controllers(
auto controller = found_it->c;
if (is_controller_active(*controller))
{
- const auto new_state = controller->get_node()->deactivate();
- controller->release_interfaces();
- // if it is a chainable controller, make the reference interfaces unavailable on deactivation
- if (controller->is_chainable())
+ try
{
- resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
+ const auto new_state = controller->get_node()->deactivate();
+ controller->release_interfaces();
+
+ // if it is a chainable controller, make the reference interfaces unavailable on
+ // deactivation
+ if (controller->is_chainable())
+ {
+ resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
+ }
+ if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+ {
+ RCLCPP_ERROR(
+ get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive",
+ controller_name.c_str(), new_state.label().c_str());
+ }
}
- if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+ catch (const std::exception & e)
{
RCLCPP_ERROR(
- get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive",
- controller_name.c_str(), new_state.label().c_str());
+ get_logger(), "Caught exception while deactivating the controller '%s': %s",
+ controller_name.c_str(), e.what());
+ continue;
+ }
+ catch (...)
+ {
+ RCLCPP_ERROR(
+ get_logger(), "Caught unknown exception while deactivating the controller '%s'",
+ controller_name.c_str());
+ continue;
}
}
}
@@ -1517,15 +1611,32 @@ void ControllerManager::activate_controllers(
}
controller->assign_interfaces(std::move(command_loans), std::move(state_loans));
- const auto new_state = controller->get_node()->activate();
- if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+ try
+ {
+ const auto new_state = controller->get_node()->activate();
+ if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+ {
+ RCLCPP_ERROR(
+ get_logger(),
+ "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).",
+ controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(),
+ hardware_interface::lifecycle_state_names::ACTIVE,
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+ }
+ }
+ catch (const std::exception & e)
{
RCLCPP_ERROR(
- get_logger(),
- "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).",
- controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(),
- hardware_interface::lifecycle_state_names::ACTIVE,
- lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+ get_logger(), "Caught exception while activating the controller '%s': %s",
+ controller_name.c_str(), e.what());
+ continue;
+ }
+ catch (...)
+ {
+ RCLCPP_ERROR(
+ get_logger(), "Caught unknown exception while activating the controller '%s'",
+ controller_name.c_str());
+ continue;
}
// if it is a chainable controller, make the reference interfaces available on activation
@@ -2037,6 +2148,7 @@ controller_interface::return_type ControllerManager::update(
++update_loop_counter_;
update_loop_counter_ %= update_rate_;
+ std::vector failed_controllers_list;
for (const auto & loaded_controller : rt_controller_list)
{
// TODO(v-lopez) we could cache this information
@@ -2049,6 +2161,17 @@ controller_interface::return_type ControllerManager::update(
run_controller_at_cm_rate ? period
: rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
+ if (
+ *loaded_controller.next_update_cycle_time ==
+ rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
+ {
+ // it is zero after activation
+ 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;
+ }
+
bool controller_go =
(time ==
rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
@@ -2063,23 +2186,50 @@ controller_interface::return_type ControllerManager::update(
{
const auto controller_actual_period =
(time - *loaded_controller.next_update_cycle_time) + controller_period;
- auto controller_ret = loaded_controller.c->update(time, controller_actual_period);
-
- if (
- *loaded_controller.next_update_cycle_time ==
- rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
+ auto controller_ret = controller_interface::return_type::OK;
+ // Catch exceptions thrown by the controller update function
+ try
+ {
+ controller_ret = loaded_controller.c->update(time, controller_actual_period);
+ }
+ catch (const std::exception & e)
{
- *loaded_controller.next_update_cycle_time = time;
+ RCLCPP_ERROR(
+ get_logger(), "Caught exception while updating controller '%s': %s",
+ loaded_controller.info.name.c_str(), e.what());
+ controller_ret = controller_interface::return_type::ERROR;
}
+ catch (...)
+ {
+ RCLCPP_ERROR(
+ get_logger(), "Caught unknown exception while updating controller '%s'",
+ loaded_controller.info.name.c_str());
+ controller_ret = controller_interface::return_type::ERROR;
+ }
+
*loaded_controller.next_update_cycle_time += controller_period;
if (controller_ret != controller_interface::return_type::OK)
{
+ failed_controllers_list.push_back(loaded_controller.info.name);
ret = controller_ret;
}
}
}
}
+ if (!failed_controllers_list.empty())
+ {
+ std::string failed_controllers;
+ for (const auto & controller : failed_controllers_list)
+ {
+ failed_controllers += "\n\t- " + controller;
+ }
+ RCLCPP_ERROR(
+ get_logger(), "Deactivating following controllers as their update resulted in an error :%s",
+ failed_controllers.c_str());
+
+ deactivate_controllers(rt_controller_list, failed_controllers_list);
+ }
// there are controllers to (de)activate
if (switch_params_.do_switch)
@@ -2187,6 +2337,13 @@ std::pair ControllerManager::split_command_interface(
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();
+}
+
void ControllerManager::propagate_deactivation_of_chained_mode(
const std::vector & controllers)
{
diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
index 2747e79a1b..6dd7d72fb2 100644
--- a/controller_manager/src/ros2_control_node.cpp
+++ b/controller_manager/src/ros2_control_node.cpp
@@ -83,6 +83,8 @@ int main(int argc, char ** argv)
next_iteration_time += period;
std::this_thread::sleep_until(next_iteration_time);
}
+
+ cm->shutdown_async_controllers_and_components();
});
executor->add_node(cm);
diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp
index 7585ae36e5..8f120bbd47 100644
--- a/controller_manager/test/test_controller/test_controller.cpp
+++ b/controller_manager/test/test_controller/test_controller.cpp
@@ -62,7 +62,7 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con
controller_interface::return_type TestController::update(
const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
- update_period_ = period.seconds();
+ update_period_ = period;
++internal_counter;
// set value to hardware to produce and test different behaviors there
@@ -76,6 +76,14 @@ controller_interface::return_type TestController::update(
{
for (size_t i = 0; i < command_interfaces_.size(); ++i)
{
+ if (!std::isfinite(external_commands_for_testing_[i]))
+ {
+ RCLCPP_ERROR(
+ get_node()->get_logger(),
+ "External command value for command interface '%s' is not finite",
+ command_interfaces_[i].get_name().c_str());
+ return controller_interface::return_type::ERROR;
+ }
RCLCPP_INFO(
get_node()->get_logger(), "Setting value of command interface '%s' to %f",
command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]);
diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp
index 14ad753803..368aeae4a8 100644
--- a/controller_manager/test/test_controller/test_controller.hpp
+++ b/controller_manager/test/test_controller/test_controller.hpp
@@ -80,7 +80,7 @@ class TestController : public controller_interface::ControllerInterface
// enables external setting of values to command interfaces - used for simulation of hardware
// errors
double set_first_command_interface_value_to;
- double update_period_ = 0;
+ rclcpp::Duration update_period_ = rclcpp::Duration::from_seconds(0.);
};
} // namespace test_controller
diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
index e88b41f222..3e2f91e91a 100644
--- a/controller_manager/test/test_controller_manager.cpp
+++ b/controller_manager/test/test_controller_manager.cpp
@@ -374,9 +374,12 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
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)
- ASSERT_GE(test_controller->update_period_, 1.0 / cm_update_rate);
- ASSERT_LT(test_controller->update_period_, 2.0 / cm_update_rate);
+ // [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))));
loop_rate.sleep();
}
// if we do 2 times of the controller_manager update rate, the internal counter should be
@@ -445,6 +448,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_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 = {};
@@ -472,6 +476,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
const auto controller_update_rate = test_controller->get_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)
{
@@ -480,8 +485,12 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
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)
- ASSERT_GE(test_controller->update_period_, 1.0 / controller_update_rate);
- ASSERT_LT(test_controller->update_period_, 2.0 / controller_update_rate);
+ EXPECT_THAT(
+ test_controller->update_period_,
+ 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;
time += rclcpp::Duration::from_seconds(0.01);
if (update_counter % cm_update_rate == 0)
diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp
index 4c800d41c2..6e2fba23db 100644
--- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp
+++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp
@@ -405,6 +405,106 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er
}
}
+TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error)
+{
+ auto strictness = GetParam().strictness;
+ SetupAndConfigureControllers(strictness);
+
+ rclcpp_lifecycle::State state_active(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+ hardware_interface::lifecycle_state_names::ACTIVE);
+
+ {
+ EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
+ EXPECT_GE(test_controller_actuator->internal_counter, 1u)
+ << "Controller is started at the end of update";
+ EXPECT_GE(test_controller_system->internal_counter, 1u)
+ << "Controller is started at the end of update";
+ EXPECT_GE(test_broadcaster_all->internal_counter, 1u)
+ << "Controller is started at the end of update";
+ EXPECT_GE(test_broadcaster_sensor->internal_counter, 1u)
+ << "Controller is started at the end of update";
+ }
+
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id());
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id());
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id());
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id());
+
+ // Execute first time without any errors
+ {
+ auto new_counter = test_controller_actuator->internal_counter + 1;
+ EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
+ EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors";
+ EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors";
+ EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors";
+ EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) << "Execute without errors";
+ }
+
+ // Simulate error in update method of the controllers but not in hardware
+ test_controller_actuator->external_commands_for_testing_[0] =
+ std::numeric_limits::quiet_NaN();
+ test_controller_system->external_commands_for_testing_[0] =
+ std::numeric_limits::quiet_NaN();
+ {
+ auto new_counter = test_controller_actuator->internal_counter + 1;
+ EXPECT_EQ(controller_interface::return_type::ERROR, cm_->update(time_, PERIOD));
+ EXPECT_EQ(test_controller_actuator->internal_counter, new_counter)
+ << "Executes the current cycle and returns ERROR";
+ EXPECT_EQ(
+ test_controller_actuator->get_state().id(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+ EXPECT_EQ(test_controller_system->internal_counter, new_counter)
+ << "Executes the current cycle and returns ERROR";
+ EXPECT_EQ(
+ test_controller_system->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+ EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter)
+ << "Execute without errors to write value";
+ EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
+ << "Execute without errors to write value";
+ }
+
+ {
+ auto previous_counter = test_controller_actuator->internal_counter;
+ auto new_counter = test_controller_system->internal_counter + 1;
+
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+ test_controller_actuator->get_state().id());
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id());
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id());
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id());
+
+ EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD));
+ EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter)
+ << "Cannot execute as it should be currently deactivated";
+ EXPECT_EQ(test_controller_system->internal_counter, previous_counter)
+ << "Cannot execute as it should be currently deactivated";
+ EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter)
+ << "Broadcaster all interfaces without errors";
+ EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter)
+ << "Execute without errors to write value";
+
+ // The states shouldn't change as there are no more controller errors
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+ test_controller_actuator->get_state().id());
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id());
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id());
+ EXPECT_EQ(
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id());
+ }
+}
+
TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_error)
{
auto strictness = GetParam().strictness;
diff --git a/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml b/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml
new file mode 100644
index 0000000000..bc93f162c1
--- /dev/null
+++ b/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml
@@ -0,0 +1,13 @@
+ctrl_1:
+ ros__parameters:
+ joint_names: ["joint1"]
+
+ctrl_2:
+ ros__parameters:
+ joint_names: ["joint2"]
+ fallback_controllers: ["ctrl_6", "ctrl_7", "ctrl_8"]
+
+ctrl_3:
+ ros__parameters:
+ joint_names: ["joint3"]
+ fallback_controllers: ["ctrl_9"]
diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml
new file mode 100644
index 0000000000..95a6efba30
--- /dev/null
+++ b/controller_manager/test/test_controller_spawner_with_type.yaml
@@ -0,0 +1,13 @@
+ctrl_with_parameters_and_type:
+ ros__parameters:
+ 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"]
+
+ctrl_with_parameters_and_no_type:
+ ros__parameters:
+ joint_names: ["joint2"]
diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp
index 3b774035aa..a5dd8fcda3 100644
--- a/controller_manager/test/test_spawner_unspawner.cpp
+++ b/controller_manager/test/test_spawner_unspawner.cpp
@@ -23,6 +23,7 @@
#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::_;
@@ -259,12 +260,63 @@ TEST_F(TestLoadController, spawner_test_type_in_arg)
ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
}
+TEST_F(TestLoadController, spawner_test_type_in_params_file)
+{
+ const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_type.yaml";
+
+ // 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 "
+ "test_controller_manager -p " +
+ test_file_path),
+ 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, "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_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, "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_state().id(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path),
+ 256);
+ // 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, "ctrl_with_parameters_and_type");
+ ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_EQ(ctrl_1.c->get_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.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+}
+
TEST_F(TestLoadController, unload_on_kill)
{
// Launch spawner with unload on kill
// timeout command will kill it after the specified time with signal SIGINT
std::stringstream ss;
- ss << "timeout --signal=INT 5 " << "ros2 run controller_manager spawner "
+ ss << "timeout --signal=INT 5 "
+ << "ros2 run controller_manager spawner "
<< "ctrl_3 -c test_controller_manager -t "
<< std::string(test_controller::TEST_CONTROLLER_CLASS_NAME) << " --unload-on-kill";
@@ -273,3 +325,58 @@ TEST_F(TestLoadController, unload_on_kill)
ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul);
}
+
+TEST_F(TestLoadController, spawner_test_fallback_controllers)
+{
+ const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_fallback_controllers.yaml";
+
+ cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+ cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+ cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+
+ ControllerManagerRunner cm_runner(this);
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 "
+ "-p " +
+ test_file_path),
+ 0);
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
+ {
+ auto ctrl_1 = cm_->get_loaded_controllers()[0];
+ ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+ ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_THAT(
+ ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
+ ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ }
+
+ // Try to spawn now the controller with fallback controllers inside the yaml
+ EXPECT_EQ(
+ call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0);
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
+ {
+ auto ctrl_1 = cm_->get_loaded_controllers()[0];
+ ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+ ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_THAT(
+ ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
+ ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+ auto ctrl_2 = cm_->get_loaded_controllers()[1];
+ ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
+ ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_THAT(
+ ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8"));
+ ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+ auto ctrl_3 = cm_->get_loaded_controllers()[2];
+ ASSERT_EQ(ctrl_3.info.name, "ctrl_3");
+ ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9"));
+ ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ }
+}
diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst
index 11d3ba2989..6cc2a09a43 100644
--- a/controller_manager_msgs/CHANGELOG.rst
+++ b/controller_manager_msgs/CHANGELOG.rst
@@ -2,6 +2,15 @@
Changelog for package controller_manager_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.11.0 (2024-05-14)
+-------------------
+
+4.10.0 (2024-05-08)
+-------------------
+
+4.9.0 (2024-04-30)
+------------------
+
4.8.0 (2024-03-27)
------------------
diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml
index db64f0c28d..ddd840154e 100644
--- a/controller_manager_msgs/package.xml
+++ b/controller_manager_msgs/package.xml
@@ -2,7 +2,7 @@
controller_manager_msgs
- 4.8.0
+ 4.11.0
Messages and services for the controller manager.
Bence Magyar
Denis Štogl
diff --git a/doc/index.rst b/doc/index.rst
index 91f965d601..b1d8e21c7d 100644
--- a/doc/index.rst
+++ b/doc/index.rst
@@ -16,12 +16,6 @@ API Documentation
API documentation is parsed by doxygen and can be found `here <../../api/index.html>`_
-=========
-Features
-=========
-
-* :ref:`Command Line Interface (CLI) `
-
========
Concepts
========
@@ -31,5 +25,6 @@ Concepts
Controller Manager <../controller_manager/doc/userdoc.rst>
Controller Chaining / Cascade Control <../controller_manager/doc/controller_chaining.rst>
+ Joint Kinematics <../hardware_interface/doc/joints_userdoc.rst>
Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst>
Mock Components <../hardware_interface/doc/mock_components_userdoc.rst>
diff --git a/doc/migration/Foxy.rst b/doc/migration/Galactic.rst
similarity index 94%
rename from doc/migration/Foxy.rst
rename to doc/migration/Galactic.rst
index a1afeddc70..4c6e958951 100644
--- a/doc/migration/Foxy.rst
+++ b/doc/migration/Galactic.rst
@@ -1,3 +1,5 @@
+:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Galactic.rst
+
Foxy to Galactic
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
diff --git a/doc/migration/Jazzy.rst b/doc/migration/Jazzy.rst
new file mode 100644
index 0000000000..1880f5d380
--- /dev/null
+++ b/doc/migration/Jazzy.rst
@@ -0,0 +1,70 @@
+:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/migration/Jazzy.rst
+
+Iron to Jazzy
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+controller_manager
+******************
+* Rename ``class_type`` to ``plugin_name`` (`#780 `_)
+* CM now subscribes to ``robot_description`` topic instead of ``~/robot_description`` (`#1410 `_). As a consequence, when using multiple controller managers, you have to remap the topic within the launch file, an example for a python launch file:
+
+ .. code-block:: python
+
+ remappings=[
+ ('/robot_description', '/custom_1/robot_description'),
+ ]
+
+* Changes from `(PR #1256) `__
+
+ * All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise the following error is shown:
+
+ The published robot description file (URDF) seems not to be genuine. The following error was caught: not found in URDF.
+
+ This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` interface types instead.
+
+ * The syntax for mimic joints is changed to the `official URDF specification `__. The parameters within the ``ros2_control`` tag are not supported any more. Instead of
+
+ .. code-block:: xml
+
+
+
+
+
+ 0.15
+
+
+
+
+
+ right_finger_joint
+ 1
+
+
+
+
+
+
+
+ define your mimic joints directly in the joint definitions:
+
+ .. code-block:: xml
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+hardware_interface
+******************
+* ``test_components`` was moved to its own package. Update the dependencies if you are using them. (`#1325 `_)
diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst
new file mode 100644
index 0000000000..4074cbca63
--- /dev/null
+++ b/doc/release_notes/Jazzy.rst
@@ -0,0 +1,109 @@
+:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Jazzy.rst
+
+Iron to Jazzy
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+General
+*******
+* A ``version.h`` file will be generated per package using the ament_generate_version_header (`#1449 `_). For example, you can include the version of the package in the logs.
+
+ .. code-block:: cpp
+
+ #include
+ ...
+ RCLCPP_INFO(get_logger(), "controller_manager version: %s", CONTROLLER_MANAGER_VERSION_STR);
+
+controller_interface
+********************
+For details see the controller_manager section.
+
+* Pass URDF to controllers on init (`#1088 `_).
+* Pass controller manager update rate on the init of the controller interface (`#1141 `_)
+* A method to get node options to setup the controller node #api-breaking (`#1169 `_)
+
+controller_manager
+******************
+* URDF is now passed to controllers on init (`#1088 `_)
+ This should help avoiding extra legwork in controllers to get access to the ``/robot_description``.
+* Pass controller manager update rate on the init of the controller interface (`#1141 `_)
+* Report inactive controllers as a diagnostics ok instead of an error (`#1184 `_)
+* Set chained controller interfaces 'available' for activated controllers (`#1098 `_)
+
+ * Configured chainable controller: Listed exported interfaces are unavailable and unclaimed
+ * Active chainable controller (not in chained mode): Listed exported interfaces are available but unclaimed
+ * Active chainable controller (in chained mode): Listed exported interfaces are available and claimed
+* Try using SCHED_FIFO on any kernel (`#1142 `_)
+* A method to get node options to setup the controller node was added (`#1169 `_): ``get_node_options`` can be overridden by controllers, this would make it easy for other controllers to be able to setup their own custom node options
+* CM now subscribes to ``robot_description`` topic instead of ``~/robot_description`` (`#1410 `_).
+* Change the controller sorting with an approach similar to directed acyclic graphs (`#1384 `_)
+* Changes from `(PR #1256) `__
+
+ * All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise the following error is shown:
+
+ The published robot description file (URDF) seems not to be genuine. The following error was caught: not found in URDF.
+
+ This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` interface types instead.
+
+ * The syntax for mimic joints is changed to the `official URDF specification `__.
+
+ .. code-block:: xml
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ The parameters within the ``ros2_control`` tag are not supported any more.
+
+hardware_interface
+******************
+* A portable version for string-to-double conversion was added: ``hardware_interface::stod`` (`#1257 `_)
+* ``test_components`` was moved to its own package (`#1325 `_)
+* The ``ros2_control`` tag now supports parsing of the limits from the URDF into the ``HardwareInfo`` structure. More conservative limits can be defined using the ``min`` and ``max`` attributes per interface (`#1472 `_)
+
+ .. code:: xml
+
+
+
+ ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware
+ 2.0
+ 3.0
+ 2.0
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+
+
+* Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_)
+
+joint_limits
+************
+* Add header to import limits from standard URDF definition (`#1298 `_)
+
+ros2controlcli
+**************
+* Spawner colours were added to ``list_controllers`` depending upon active or inactive (`#1409 `_)
+* The ``set_hardware_component_state`` verb was added (`#1248 `_). Use the following command to set the state of a hardware component
+
+ .. code-block:: bash
+
+ ros2 control set_hardware_component_state
diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst
index 1cb22a501a..77185ecf28 100644
--- a/hardware_interface/CHANGELOG.rst
+++ b/hardware_interface/CHANGELOG.rst
@@ -2,6 +2,27 @@
Changelog for package hardware_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.11.0 (2024-05-14)
+-------------------
+* Add find_package for ament_cmake_gen_version_h (`#1534 `_)
+* Parse URDF soft_limits into the HardwareInfo structure (`#1488 `_)
+* Contributors: Christoph Fröhlich, adriaroig
+
+4.10.0 (2024-05-08)
+-------------------
+* Add hardware components exception handling in resource manager (`#1508 `_)
+* Working async controllers and components [not synchronized] (`#1041 `_)
+* Parse URDF joint hard limits into the HardwareInfo structure (`#1472 `_)
+* Add fallback controllers list to the ControllerInfo (`#1503 `_)
+* Add more common hardware interface type constants (`#1500 `_)
+* Contributors: Márk Szitanics, Sai Kishor Kothakota
+
+4.9.0 (2024-04-30)
+------------------
+* Add missing calculate_dynamics (`#1498 `_)
+* Component parser: Get mimic information from URDF (`#1256 `_)
+* Contributors: Christoph Fröhlich
+
4.8.0 (2024-03-27)
------------------
* generate version.h file per package using the ament_generate_version_header (`#1449 `_)
diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt
index 11938f0253..8c22947b96 100644
--- a/hardware_interface/CMakeLists.txt
+++ b/hardware_interface/CMakeLists.txt
@@ -15,10 +15,12 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
rcutils
TinyXML2
tinyxml2_vendor
+ joint_limits
urdf
)
find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_gen_version_h REQUIRED)
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach()
diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst
index ecf852cb94..d8338bf7a6 100644
--- a/hardware_interface/doc/hardware_interface_types_userdoc.rst
+++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst
@@ -41,6 +41,12 @@ The ```` tag can be used as a child of all three types of hardware compone
Because ports implemented as ````-tag are typically very application-specific, there exists no generic publisher
within the ros2_control framework. A custom gpio-controller has to be implemented for each application. As an example, see :ref:`the GPIO controller example ` as part of the demo repository.
+Hardware Groups
+*****************************
+Hardware Component Groups serve as a critical organizational mechanism within complex systems, facilitating error handling and fault tolerance. By grouping related hardware components together, such as actuators within a manipulator, users can establish a unified framework for error detection and response.
+
+Hardware Component Groups play a vital role in propagating errors across interconnected hardware components. For instance, in a manipulator system, grouping actuators together allows for error propagation. If one actuator fails within the group, the error can propagate to the other actuators, signaling a potential issue across the system. By default, the actuator errors are isolated to their own hardware component, allowing the rest to continue operation unaffected. In the provided ros2_control configuration, the ```` tag within each ```` block signifies the grouping of hardware components, enabling error propagation mechanisms within the system.
+
Examples
*****************************
The following examples show how to use the different hardware interface types in a ``ros2_control`` URDF.
@@ -152,3 +158,66 @@ They can be combined together within the different hardware component types (sys
+
+4. Robot with multiple hardware components belonging to same group : ``Group1``
+
+ - RRBot System 1 and 2
+ - Digital: Total 4 inputs and 2 outputs
+ - Analog: Total 2 inputs and 1 output
+ - Vacuum valve at the flange (on/off)
+ - Group: Group1
+
+ .. code:: xml
+
+
+
+ ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware
+ Group1
+ 2.0
+ 3.0
+ 2.0
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+ 3.1
+
+
+
+
+
+
+
+
+
+
+
+ ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware
+ Group1
+ 2.0
+ 3.0
+ 2.0
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/hardware_interface/doc/joints_userdoc.rst b/hardware_interface/doc/joints_userdoc.rst
new file mode 100644
index 0000000000..f4332410d5
--- /dev/null
+++ b/hardware_interface/doc/joints_userdoc.rst
@@ -0,0 +1,132 @@
+:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/joints_userdoc.rst
+
+.. _joints_userdoc:
+
+
+Joint Kinematics for ros2_control
+---------------------------------------------------------
+
+This page should give an overview of the joint kinematics in the context of ros2_control. It is intended to give a brief introduction to the topic and to explain the current implementation in ros2_control.
+
+Nomenclature
+############
+
+Degrees of Freedom (DoF)
+ From `wikipedia `__:
+
+ In physics, the degrees of freedom (DoF) of a mechanical system is the number of independent parameters that define its configuration or state.
+
+Joint
+ A joint is a connection between two links. In the ROS ecosystem, three types are more typical: Revolute (A hinge joint with position limits), Continuous (A continuous hinge without any position limits) or Prismatic (A sliding joint that moves along the axis).
+
+ In general, a joint can be actuated or non-actuated, also called passive. Passive joints are joints that do not have their own actuation mechanism but instead allow movement by external forces or by being passively moved by other joints. A passive joint can have a DoF of one, such as a pendulum, or it can be part of a parallel kinematic mechanism with zero DoF.
+
+Serial Kinematics
+ Serial kinematics refers to the arrangement of joints in a robotic manipulator where each joint is independent of the others, and the number of joints is equal to the DoF of the kinematic chain.
+
+ A typical example is an industrial robot with six revolute joints, having 6-DoF. Each joint can be actuated independently, and the end-effector can be moved to any position and orientation in the workspace.
+
+Kinematic Loops
+ On the other hand, kinematic loops, also known as closed-loop mechanisms, involve several joints that are connected in a kinematic chain and being actuated together. This means that the joints are coupled and cannot be moved independently: In general, the number of DoFs is smaller than the number of joints.
+ This structure is typical for parallel kinematic mechanisms, where the end-effector is connected to the base by several kinematic chains.
+
+ An example is the four-bar linkage, which consists of four links and four joints. It can have one or two actuators and consequently one or two DoFs, despite having four joints. Furthermore, we can say that we have one (two) actuated joint and three (two) passive joints, which must satisfy the kinematic constraints of the mechanism.
+
+URDF
+#############
+
+URDF is the default format to describe robot kinematics in ROS. However, only serial kinematic chains are supported, except for the so-called mimic joints. See the `URDF specification `__ for more details.
+
+Mimic joints can be defined in the following way in the URDF
+
+ .. code-block:: xml
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+Mimic joints are an abstraction of the real world. For example, they can be used to describe
+
+* simple closed-loop kinematics with linear dependencies of the joint positions and velocities
+* links connected with belts, like belt and pulley systems or telescope arms
+* a simplified model of passive joints, e.g. a pendulum at the end-effector always pointing downwards
+* abstract complex groups of actuated joints, where several joints are directly controlled by low-level control loops and move synchronously. Without giving a real-world example, this could be several motors with their individual power electronics but commanded with the same setpoint.
+
+Mimic joints defined in the URDF are parsed from the resource manager and stored in a class variable of type ``HardwareInfo``, which can be accessed by the hardware components. The mimic joints must not have command interfaces but can have state interfaces.
+
+.. code-block:: xml
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+From the officially released packages, the following packages are already using this information:
+
+* :ref:`mock_components (generic system) `
+* :ref:`gazebo_ros2_control `
+* :ref:`gz_ros2_control `
+
+As the URDF specifies only the kinematics, the mimic tag has to be independent of the hardware interface type used in ros2_control. This means that we interpret this info in the following way:
+
+* **position = multiplier * other_joint_position + offset**
+* **velocity = multiplier * other_joint_velocity**
+
+If someone wants to deactivate the mimic joint behavior for whatever reason without changing the URDF, it can be done by setting the attribute ``mimic=false`` of the joint tag in the ```` section of the XML.
+
+.. code-block:: xml
+
+
+
+
+
+
+
+Transmission Interface
+#######################
+Mechanical transmissions transform effort/flow variables such that their product (power) remains constant. Effort variables for linear and rotational domains are force and torque; while the flow variables are respectively linear velocity and angular velocity.
+
+In robotics it is customary to place transmissions between actuators and joints. This interface adheres to this naming to identify the input and output spaces of the transformation. The provided interfaces allow bidirectional mappings between actuator and joint spaces for effort, velocity and position. Position is not a power variable, but the mappings can be implemented using the velocity map plus an integration constant representing the offset between actuator and joint zeros.
+
+The ``transmission_interface`` provides a base class and some implementations for plugins, which can be integrated and loaded by custom hardware components. They are not automatically loaded by any hardware component or the gazebo plugins, each hardware component is responsible for loading the appropriate transmission interface to map the actuator readings to joint readings.
+
+Currently the following implementations are available:
+
+* ``SimpleTransmission``: A simple transmission with a constant reduction ratio and no additional dynamics.
+* ``DifferentialTransmission``: A differential transmission with two actuators and two joints.
+* ``FourBarLinkageTransmission``: A four-bar-linkage transmission with two actuators and two joints.
+
+For more information, see :ref:`example_8 ` or the `transmission_interface `__ documentation.
+
+Simulating Closed-Loop Kinematic Chains
+#######################################
+Depending on the simulation plugin, different approaches can be used to simulate closed-loop kinematic chains. The following list gives an overview of the available simulation plugins and their capabilities:
+
+gazebo_ros2_control:
+ * mimic joints
+ * closed-loop kinematics are supported with ```` tags in the URDF, see, e.g., `here `__.
+
+gz_ros2_control:
+ * mimic joints
+ * closed-loop kinematics are not directly supported yet, but can be implemented by using a ``DetachableJoint`` via custom plugins. Follow `this issue `__ for updates on this topic.
diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst
index 782d3e01ea..8eca65c52b 100644
--- a/hardware_interface/doc/mock_components_userdoc.rst
+++ b/hardware_interface/doc/mock_components_userdoc.rst
@@ -28,27 +28,74 @@ Features:
Parameters
,,,,,,,,,,
+A full example including all optional parameters (with default values):
+
+.. code-block:: xml
+
+
+
+ mock_components/GenericSystem
+ false
+
+ false
+ false
+ false
+ 0.0
+
+
+
+
+
+ 3.45
+
+
+
+
+
+
+
+
+ 2.78
+
+
+
+
+
+
+
+
+
+
+
+See :ref:`example_2 ` for an example using ``calculate_dynamics`` or :ref:`example_10 ` for using in combination with GPIO interfaces.
+
+Component Parameters
+####################
+
+calculate_dynamics (optional; boolean; default: false)
+ Calculation of states from commands by using Euler-forward integration or finite differences.
+
+custom_interface_with_following_offset (optional; string; default: "")
+ Mapping of offsetted commands to a custom interface.
+
disable_commands (optional; boolean; default: false)
Disables mirroring commands to states.
This option is helpful to simulate an erroneous connection to the hardware when nothing breaks, but suddenly there is no feedback from a hardware interface.
Or it can help you to test your setup when the hardware is running without feedback, i.e., in open loop configuration.
-mock_sensor_commands (optional; boolean; default: false)
- Creates fake command interfaces for faking sensor measurements with an external command.
- Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world.
-
mock_gpio_commands (optional; boolean; default: false)
Creates fake command interfaces for faking GPIO states with an external command.
Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world.
-position_state_following_offset (optional; double; default: 0.0)
- Following offset added to the commanded values when mirrored to states.
+mock_sensor_commands (optional; boolean; default: false)
+ Creates fake command interfaces for faking sensor measurements with an external command.
+ Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world.
-custom_interface_with_following_offset (optional; string; default: "")
- Mapping of offsetted commands to a custom interface.
+position_state_following_offset (optional; double; default: 0.0)
+ Following offset added to the commanded values when mirrored to states. Only applied, if ``custom_interface_with_following_offset`` is false.
-Per-interface Parameters
-,,,,,,,,,,,,,,,,,,,,,,,,
+Per-Interface Parameters
+########################
initial_value (optional; double)
Initial value of certain state interface directly after startup. Example:
diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp
index b3c668bf38..2ff7fc0dbd 100644
--- a/hardware_interface/include/hardware_interface/actuator.hpp
+++ b/hardware_interface/include/hardware_interface/actuator.hpp
@@ -83,6 +83,9 @@ class Actuator final
HARDWARE_INTERFACE_PUBLIC
std::string get_name() const;
+ HARDWARE_INTERFACE_PUBLIC
+ std::string get_group_name() const;
+
HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & get_state() const;
diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp
index 30961316e7..06cd0be869 100644
--- a/hardware_interface/include/hardware_interface/actuator_interface.hpp
+++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp
@@ -295,7 +295,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
* \note All starting and stopping interface keys are passed to all components, so the function
* should return return_type::OK by default when given interface keys not relevant for this
* component. \param[in] start_interfaces vector of string identifiers for the command interfaces
- * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs
+ * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
* stopping. \return return_type::OK if the new command interface combination can be prepared, or
* if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
*/
@@ -314,7 +314,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
* \note All starting and stopping interface keys are passed to all components, so the function
* should return return_type::OK by default when given interface keys not relevant for this
* component. \param[in] start_interfaces vector of string identifiers for the command interfaces
- * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs
+ * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
* stopping. \return return_type::OK if the new command interface combination can be switched to,
* or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
*/
@@ -354,6 +354,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
*/
virtual std::string get_name() const { return info_.name; }
+ /// Get name of the actuator hardware group to which it belongs to.
+ /**
+ * \return group name.
+ */
+ virtual std::string get_group_name() const { return info_.group; }
+
/// Get life-cycle state of the actuator hardware.
/**
* \return state.
diff --git a/hardware_interface/include/hardware_interface/async_components.hpp b/hardware_interface/include/hardware_interface/async_components.hpp
index 2fa6fd7f85..73495c92f8 100644
--- a/hardware_interface/include/hardware_interface/async_components.hpp
+++ b/hardware_interface/include/hardware_interface/async_components.hpp
@@ -17,6 +17,7 @@
#include
#include
+#include
#include
#include "hardware_interface/actuator.hpp"
@@ -34,29 +35,23 @@ class AsyncComponentThread
{
public:
explicit AsyncComponentThread(
- Actuator * component, unsigned int update_rate,
+ unsigned int update_rate,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
- : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface)
+ : cm_update_rate_(update_rate), clock_interface_(clock_interface)
{
}
- explicit AsyncComponentThread(
- System * component, unsigned int update_rate,
- rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
- : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface)
- {
- }
-
- explicit AsyncComponentThread(
- Sensor * component, unsigned int update_rate,
- rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
- : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface)
+ // Fills the internal variant with the desired component.
+ template
+ void register_component(T * component)
{
+ hardware_component_ = component;
}
AsyncComponentThread(const AsyncComponentThread & t) = delete;
- AsyncComponentThread(AsyncComponentThread && t) = default;
+ AsyncComponentThread(AsyncComponentThread && t) = delete;
+ // Destructor, called when the component is erased from its map.
~AsyncComponentThread()
{
terminated_.store(true, std::memory_order_seq_cst);
@@ -65,9 +60,19 @@ class AsyncComponentThread
write_and_read_.join();
}
}
-
+ /// Creates the component's thread.
+ /**
+ * Called when the component is activated.
+ *
+ */
void activate() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); }
+ /// Periodically execute the component's write and read methods.
+ /**
+ * Callback of the async component's thread.
+ * **Not synchronized with the controller manager's update currently**
+ *
+ */
void write_and_read()
{
using TimePoint = std::chrono::system_clock::time_point;
@@ -88,8 +93,12 @@ class AsyncComponentThread
auto measured_period = current_time - previous_time;
previous_time = current_time;
- // write
- // read
+ if (!first_iteration)
+ {
+ component->write(clock_interface_->get_clock()->now(), measured_period);
+ }
+ component->read(clock_interface_->get_clock()->now(), measured_period);
+ first_iteration = false;
}
next_iteration_time += period;
std::this_thread::sleep_until(next_iteration_time);
@@ -104,6 +113,7 @@ class AsyncComponentThread
std::thread write_and_read_{};
unsigned int cm_update_rate_;
+ bool first_iteration = true;
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
};
diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp
index 61a51a134a..a38fb99cb3 100644
--- a/hardware_interface/include/hardware_interface/controller_info.hpp
+++ b/hardware_interface/include/hardware_interface/controller_info.hpp
@@ -38,6 +38,9 @@ struct ControllerInfo
/// List of claimed interfaces by the controller.
std::vector claimed_interfaces;
+
+ /// List of fallback controller names to be activated if this controller fails.
+ std::vector fallback_controllers_names;
};
} // namespace hardware_interface
diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp
index 45afebdb34..e7d47bcaa4 100644
--- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp
+++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp
@@ -39,6 +39,9 @@ struct HardwareComponentInfo
/// Component "classification": "actuator", "sensor" or "system"
std::string type;
+ /// Component group
+ std::string group;
+
/// Component pluginlib plugin name.
std::string plugin_name;
diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp
index 6399b75c88..6abc8b56d7 100644
--- a/hardware_interface/include/hardware_interface/hardware_info.hpp
+++ b/hardware_interface/include/hardware_interface/hardware_info.hpp
@@ -19,6 +19,8 @@
#include
#include
+#include "joint_limits/joint_limits.hpp"
+
namespace hardware_interface
{
/**
@@ -160,6 +162,8 @@ struct HardwareInfo
std::string name;
/// Type of the hardware: actuator, sensor or system.
std::string type;
+ /// Hardware group to which the hardware belongs.
+ std::string group;
/// Component is async
bool is_async;
/// Name of the pluginlib plugin of the hardware that will be loaded.
@@ -194,6 +198,17 @@ struct HardwareInfo
* The XML contents prior to parsing
*/
std::string original_xml;
+ /**
+ * The URDF parsed limits of the hardware components joint command interfaces
+ */
+ std::unordered_map limits;
+
+ /**
+ * Map of software joint limits used for clamping the command where the key is the joint name.
+ * Optional If not specified or less restrictive than the JointLimits uses the previous
+ * JointLimits.
+ */
+ std::unordered_map soft_limits;
};
} // namespace hardware_interface
diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp
index a113d2db17..95c5d31283 100644
--- a/hardware_interface/include/hardware_interface/resource_manager.hpp
+++ b/hardware_interface/include/hardware_interface/resource_manager.hpp
@@ -354,7 +354,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
* \note it is assumed that `prepare_command_mode_switch` is called just before this method
* with the same input arguments.
* \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
- * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
+ * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping.
* \return true if switch is performed, false if a component rejects switching.
*/
bool perform_command_mode_switch(
@@ -377,6 +377,12 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
return_type set_component_state(
const std::string & component_name, rclcpp_lifecycle::State & target_state);
+ /// Deletes all async components from the resource storage
+ /**
+ * Needed to join the threads immediately after the control loop is ended.
+ */
+ void shutdown_async_components();
+
/// Reads all loaded hardware components.
/**
* Reads from all active hardware components.
diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp
index dd6579f66e..bc96b913a9 100644
--- a/hardware_interface/include/hardware_interface/sensor.hpp
+++ b/hardware_interface/include/hardware_interface/sensor.hpp
@@ -71,6 +71,9 @@ class Sensor final
HARDWARE_INTERFACE_PUBLIC
std::string get_name() const;
+ HARDWARE_INTERFACE_PUBLIC
+ std::string get_group_name() const;
+
HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & get_state() const;
diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp
index 2eeeba3482..6057267ead 100644
--- a/hardware_interface/include/hardware_interface/sensor_interface.hpp
+++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp
@@ -215,6 +215,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*/
virtual std::string get_name() const { return info_.name; }
+ /// Get name of the actuator hardware group to which it belongs to.
+ /**
+ * \return group name.
+ */
+ virtual std::string get_group_name() const { return info_.group; }
+
/// Get life-cycle state of the actuator hardware.
/**
* \return state.
diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp
index f74ff5a7f7..ac5b4649a4 100644
--- a/hardware_interface/include/hardware_interface/system.hpp
+++ b/hardware_interface/include/hardware_interface/system.hpp
@@ -84,6 +84,9 @@ class System final
HARDWARE_INTERFACE_PUBLIC
std::string get_name() const;
+ HARDWARE_INTERFACE_PUBLIC
+ std::string get_group_name() const;
+
HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & get_state() const;
diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
index a5dd048215..864f6f44b2 100644
--- a/hardware_interface/include/hardware_interface/system_interface.hpp
+++ b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -335,7 +335,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
* \note All starting and stopping interface keys are passed to all components, so the function
* should return return_type::OK by default when given interface keys not relevant for this
* component. \param[in] start_interfaces vector of string identifiers for the command interfaces
- * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs
+ * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
* stopping. \return return_type::OK if the new command interface combination can be prepared, or
* if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
*/
@@ -354,7 +354,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
* \note All starting and stopping interface keys are passed to all components, so the function
* should return return_type::OK by default when given interface keys not relevant for this
* component. \param[in] start_interfaces vector of string identifiers for the command interfaces
- * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs
+ * starting. \param[in] stop_interfaces vector of string identifiers for the command interfaces
* stopping. \return return_type::OK if the new command interface combination can be switched to,
* or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
*/
@@ -394,6 +394,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
*/
virtual std::string get_name() const { return info_.name; }
+ /// Get name of the actuator hardware group to which it belongs to.
+ /**
+ * \return group name.
+ */
+ virtual std::string get_group_name() const { return info_.group; }
+
/// Get life-cycle state of the actuator hardware.
/**
* \return state.
diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp
index 27b5207a0f..f6f6e052e3 100644
--- a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp
+++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp
@@ -25,6 +25,28 @@ constexpr char HW_IF_VELOCITY[] = "velocity";
constexpr char HW_IF_ACCELERATION[] = "acceleration";
/// Constant defining effort interface name
constexpr char HW_IF_EFFORT[] = "effort";
+/// Constant defining torque interface name
+constexpr char HW_IF_TORQUE[] = "torque";
+/// Constant defining force interface name
+constexpr char HW_IF_FORCE[] = "force";
+/// Constant defining current interface name
+constexpr char HW_IF_CURRENT[] = "current";
+/// Constant defining temperature interface name
+constexpr char HW_IF_TEMPERATURE[] = "temperature";
+
+/// Gains interface constants
+/// Constant defining proportional gain interface name
+constexpr char HW_IF_PROPORTIONAL_GAIN[] = "proportional";
+/// Constant defining integral gain interface name
+constexpr char HW_IF_INTEGRAL_GAIN[] = "integral";
+/// Constant defining derivative gain interface name
+constexpr char HW_IF_DERIVATIVE_GAIN[] = "derivative";
+/// Constant defining integral clamp interface name
+constexpr char HW_IF_INTEGRAL_CLAMP_MAX[] = "integral_clamp_max";
+/// Constant defining integral clamp interface name
+constexpr char HW_IF_INTEGRAL_CLAMP_MIN[] = "integral_clamp_min";
+/// Constant defining the feedforward interface name
+constexpr char HW_IF_FEEDFORWARD[] = "feedforward";
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_
diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml
index 41daac50cd..f3aaad3306 100644
--- a/hardware_interface/package.xml
+++ b/hardware_interface/package.xml
@@ -1,7 +1,7 @@
hardware_interface
- 4.8.0
+ 4.11.0
ros2_control hardware interface
Bence Magyar
Denis Štogl
@@ -16,6 +16,7 @@
rclcpp_lifecycle
rcpputils
tinyxml2_vendor
+ joint_limits
urdf
rcutils
diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp
index 76ac732e34..d5fbb520d5 100644
--- a/hardware_interface/src/actuator.cpp
+++ b/hardware_interface/src/actuator.cpp
@@ -259,6 +259,8 @@ return_type Actuator::perform_command_mode_switch(
std::string Actuator::get_name() const { return impl_->get_name(); }
+std::string Actuator::get_group_name() const { return impl_->get_group_name(); }
+
const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_state(); }
return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period)
diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp
index 3d1a20d214..c0250d418a 100644
--- a/hardware_interface/src/component_parser.cpp
+++ b/hardware_interface/src/component_parser.cpp
@@ -26,6 +26,8 @@
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/lexical_casts.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "joint_limits/joint_limits_urdf.hpp"
namespace
{
@@ -34,6 +36,7 @@ constexpr const auto kROS2ControlTag = "ros2_control";
constexpr const auto kHardwareTag = "hardware";
constexpr const auto kPluginNameTag = "plugin";
constexpr const auto kParamTag = "param";
+constexpr const auto kGroupTag = "group";
constexpr const auto kActuatorTag = "actuator";
constexpr const auto kJointTag = "joint";
constexpr const auto kSensorTag = "sensor";
@@ -43,6 +46,8 @@ constexpr const auto kCommandInterfaceTag = "command_interface";
constexpr const auto kStateInterfaceTag = "state_interface";
constexpr const auto kMinTag = "min";
constexpr const auto kMaxTag = "max";
+constexpr const auto kLimitsTag = "limits";
+constexpr const auto kEnableAttribute = "enable";
constexpr const auto kInitialValueTag = "initial_value";
constexpr const auto kMimicAttribute = "mimic";
constexpr const auto kDataTypeAttribute = "data_type";
@@ -289,6 +294,15 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml(
interface.max = interface_param->second;
}
+ // Option enable or disable the interface limits, by default they are enabled
+ interface.enable_limits = true;
+ const auto * limits_it = interfaces_it->FirstChildElement(kLimitsTag);
+ if (limits_it)
+ {
+ interface.enable_limits =
+ parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name()));
+ }
+
// Optional initial_value attribute
interface_param = interface_params.find(kInitialValueTag);
if (interface_param != interface_params.end())
@@ -340,11 +354,21 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it
}
}
+ // Option enable or disable the interface limits, by default they are enabled
+ bool enable_limits = true;
+ const auto * limits_it = component_it->FirstChildElement(kLimitsTag);
+ if (limits_it)
+ {
+ enable_limits = parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name()));
+ }
+
// Parse all command interfaces
const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag);
while (command_interfaces_it)
{
- component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it));
+ InterfaceInfo cmd_info = parse_interfaces_from_xml(command_interfaces_it);
+ cmd_info.enable_limits &= enable_limits;
+ component.command_interfaces.push_back(cmd_info);
command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag);
}
@@ -352,7 +376,9 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it
const auto * state_interfaces_it = component_it->FirstChildElement(kStateInterfaceTag);
while (state_interfaces_it)
{
- component.state_interfaces.push_back(parse_interfaces_from_xml(state_interfaces_it));
+ InterfaceInfo state_info = parse_interfaces_from_xml(state_interfaces_it);
+ state_info.enable_limits &= enable_limits;
+ component.state_interfaces.push_back(state_info);
state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag);
}
@@ -560,6 +586,11 @@ HardwareInfo parse_resource_from_xml(
const auto * type_it = ros2_control_child_it->FirstChildElement(kPluginNameTag);
hardware.hardware_plugin_name =
get_text_for_element(type_it, std::string("hardware ") + kPluginNameTag);
+ const auto * group_it = ros2_control_child_it->FirstChildElement(kGroupTag);
+ if (group_it)
+ {
+ hardware.group = get_text_for_element(group_it, std::string("hardware.") + kGroupTag);
+ }
const auto * params_it = ros2_control_child_it->FirstChildElement(kParamTag);
if (params_it)
{
@@ -596,6 +627,171 @@ HardwareInfo parse_resource_from_xml(
return hardware;
}
+/**
+ * @brief Retrieve the min and max values from the interface tag.
+ * @param itf The interface tag to retrieve the values from.
+ * @param min The minimum value to be retrieved.
+ * @param max The maximum value to be retrieved.
+ * @return true if the values are retrieved, false otherwise.
+ */
+bool retrieve_min_max_interface_values(const InterfaceInfo & itf, double & min, double & max)
+{
+ try
+ {
+ if (itf.min.empty() && itf.max.empty())
+ {
+ // If the limits don't exist then return false as they are not retrieved
+ return false;
+ }
+ if (!itf.min.empty())
+ {
+ min = hardware_interface::stod(itf.min);
+ }
+ if (!itf.max.empty())
+ {
+ max = hardware_interface::stod(itf.max);
+ }
+ return true;
+ }
+ catch (const std::invalid_argument & err)
+ {
+ std::cerr << "Error parsing the limits for the interface: " << itf.name << " from the tags ["
+ << kMinTag << ": '" << itf.min << "' and " << kMaxTag << ": '" << itf.max
+ << "'] within " << kROS2ControlTag << " tag inside the URDF. Skipping it"
+ << std::endl;
+ return false;
+ }
+}
+
+/**
+ * @brief Set custom values for acceleration and jerk limits (no URDF standard)
+ * @param itr The interface tag to retrieve the values from.
+ * @param limits The joint limits to be set.
+ */
+void set_custom_interface_values(const InterfaceInfo & itr, joint_limits::JointLimits & limits)
+{
+ if (itr.name == hardware_interface::HW_IF_ACCELERATION)
+ {
+ double max_decel(std::numeric_limits::quiet_NaN()),
+ max_accel(std::numeric_limits::quiet_NaN());
+ if (detail::retrieve_min_max_interface_values(itr, max_decel, max_accel))
+ {
+ if (std::isfinite(max_decel))
+ {
+ limits.max_deceleration = std::fabs(max_decel);
+ if (!std::isfinite(max_accel))
+ {
+ limits.max_acceleration = std::fabs(limits.max_deceleration);
+ }
+ limits.has_deceleration_limits = itr.enable_limits;
+ }
+ if (std::isfinite(max_accel))
+ {
+ limits.max_acceleration = max_accel;
+
+ if (!std::isfinite(limits.max_deceleration))
+ {
+ limits.max_deceleration = std::fabs(limits.max_acceleration);
+ }
+ limits.has_acceleration_limits = itr.enable_limits;
+ }
+ }
+ }
+ else if (itr.name == "jerk")
+ {
+ if (!itr.min.empty())
+ {
+ std::cerr << "Error parsing the limits for the interface: " << itr.name
+ << " from the tag: " << kMinTag << " within " << kROS2ControlTag
+ << " tag inside the URDF. Jerk only accepts max limits." << std::endl;
+ }
+ double min_jerk(std::numeric_limits::quiet_NaN()),
+ max_jerk(std::numeric_limits::quiet_NaN());
+ if (
+ !itr.max.empty() && detail::retrieve_min_max_interface_values(itr, min_jerk, max_jerk) &&
+ std::isfinite(max_jerk))
+ {
+ limits.max_jerk = std::abs(max_jerk);
+ limits.has_jerk_limits = itr.enable_limits;
+ }
+ }
+ else
+ {
+ if (!itr.min.empty() || !itr.max.empty())
+ {
+ std::cerr << "Unable to parse the limits for the interface: " << itr.name
+ << " from the tags [" << kMinTag << " and " << kMaxTag << "] within "
+ << kROS2ControlTag
+ << " tag inside the URDF. Supported interfaces for joint limits are: "
+ "position, velocity, effort, acceleration and jerk."
+ << std::endl;
+ }
+ }
+}
+
+/**
+ * @brief Retrieve the limits from ros2_control command interface tags and override URDF limits if
+ * restrictive
+ * @param interfaces The interfaces to retrieve the limits from.
+ * @param limits The joint limits to be set.
+ */
+void update_interface_limits(
+ const std::vector & interfaces, joint_limits::JointLimits & limits)
+{
+ for (auto & itr : interfaces)
+ {
+ if (itr.name == hardware_interface::HW_IF_POSITION)
+ {
+ limits.min_position = limits.has_position_limits && itr.enable_limits
+ ? limits.min_position
+ : -std::numeric_limits::max();
+ limits.max_position = limits.has_position_limits && itr.enable_limits
+ ? limits.max_position
+ : std::numeric_limits::max();
+ double min_pos(limits.min_position), max_pos(limits.max_position);
+ if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_pos, max_pos))
+ {
+ limits.min_position = std::max(min_pos, limits.min_position);
+ limits.max_position = std::min(max_pos, limits.max_position);
+ limits.has_position_limits = true;
+ }
+ limits.has_position_limits &= itr.enable_limits;
+ }
+ else if (itr.name == hardware_interface::HW_IF_VELOCITY)
+ {
+ limits.max_velocity =
+ limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits::max();
+ // Apply the most restrictive one in the case
+ double min_vel(-limits.max_velocity), max_vel(limits.max_velocity);
+ if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_vel, max_vel))
+ {
+ max_vel = std::min(std::abs(min_vel), max_vel);
+ limits.max_velocity = std::min(max_vel, limits.max_velocity);
+ limits.has_velocity_limits = true;
+ }
+ limits.has_velocity_limits &= itr.enable_limits;
+ }
+ else if (itr.name == hardware_interface::HW_IF_EFFORT)
+ {
+ limits.max_effort =
+ limits.has_effort_limits ? limits.max_effort : std::numeric_limits::max();
+ // Apply the most restrictive one in the case
+ double min_eff(-limits.max_effort), max_eff(limits.max_effort);
+ if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_eff, max_eff))
+ {
+ max_eff = std::min(std::abs(min_eff), max_eff);
+ limits.max_effort = std::min(max_eff, limits.max_effort);
+ limits.has_effort_limits = true;
+ }
+ limits.has_effort_limits &= itr.enable_limits;
+ }
+ else
+ {
+ detail::set_custom_interface_values(itr, limits);
+ }
+ }
+}
+
} // namespace detail
std::vector parse_control_resources_from_urdf(const std::string & urdf)
@@ -727,6 +923,37 @@ std::vector parse_control_resources_from_urdf(const std::string &
hw_info.mimic_joints.push_back(mimic_joint);
}
}
+ // TODO(christophfroehlich) remove this code if deprecated mimic attribute - branch
+ // from above is removed (double check here, but throws already above if not found in URDF)
+ auto urdf_joint = model.getJoint(joint.name);
+ if (!urdf_joint)
+ {
+ std::cerr << "Joint: '" + joint.name + "' not found in URDF. Skipping limits parsing!"
+ << std::endl;
+ continue;
+ }
+ if (urdf_joint->type == urdf::Joint::FIXED)
+ {
+ throw std::runtime_error(
+ "Joint '" + joint.name +
+ "' is of type 'fixed'. "
+ "Fixed joints do not make sense in ros2_control.");
+ }
+ joint_limits::JointLimits limits;
+ getJointLimits(urdf_joint, limits);
+ // Take the most restricted one. Also valid for continuous-joint type only
+ detail::update_interface_limits(joint.command_interfaces, limits);
+ hw_info.limits[joint.name] = limits;
+ joint_limits::SoftJointLimits soft_limits;
+ if (getSoftJointLimits(urdf_joint, soft_limits))
+ {
+ if (limits.has_position_limits)
+ {
+ soft_limits.min_position = std::max(soft_limits.min_position, limits.min_position);
+ soft_limits.max_position = std::min(soft_limits.max_position, limits.max_position);
+ }
+ hw_info.soft_limits[joint.name] = soft_limits;
+ }
}
}
diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp
index 300a0e6cf0..055ccc4eb8 100644
--- a/hardware_interface/src/mock_components/generic_system.cpp
+++ b/hardware_interface/src/mock_components/generic_system.cpp
@@ -459,7 +459,8 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
return return_type::OK;
}
- auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0)
+ auto mirror_command_to_state =
+ [](auto & states_, auto commands_, size_t start_index = 0) -> return_type
{
for (size_t i = start_index; i < states_.size(); ++i)
{
@@ -469,8 +470,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
{
states_[i][j] = commands_[i][j];
}
+ if (std::isinf(commands_[i][j]))
+ {
+ return return_type::ERROR;
+ }
}
}
+ return return_type::OK;
};
for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j)
@@ -561,13 +567,11 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
// do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position,
// velocity, and acceleration interface
- if (calculate_dynamics_)
- {
- mirror_command_to_state(joint_states_, joint_commands_, 3);
- }
- else
+ if (
+ mirror_command_to_state(joint_states_, joint_commands_, calculate_dynamics_ ? 3 : 1) !=
+ return_type::OK)
{
- mirror_command_to_state(joint_states_, joint_commands_, 1);
+ return return_type::ERROR;
}
for (const auto & mimic_joint : info_.mimic_joints)
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
index 0fa1c15e5d..d6827497b5 100644
--- a/hardware_interface/src/resource_manager.cpp
+++ b/hardware_interface/src/resource_manager.cpp
@@ -67,6 +67,26 @@ auto trigger_and_print_hardware_state_transition =
return result;
};
+std::string interfaces_to_string(
+ const std::vector & start_interfaces,
+ const std::vector & stop_interfaces)
+{
+ std::stringstream ss;
+ ss << "Start interfaces: " << std::endl << "[" << std::endl;
+ for (const auto & start_if : start_interfaces)
+ {
+ ss << " " << start_if << std::endl;
+ }
+ ss << "]" << std::endl;
+ ss << "Stop interfaces: " << std::endl << "[" << std::endl;
+ for (const auto & stop_if : stop_interfaces)
+ {
+ ss << " " << stop_if << std::endl;
+ }
+ ss << "]" << std::endl;
+ return ss.str();
+};
+
class ResourceStorage
{
static constexpr const char * pkg_name = "hardware_interface";
@@ -90,27 +110,64 @@ class ResourceStorage
}
template
- void load_hardware(
+ [[nodiscard]] bool load_hardware(
const HardwareInfo & hardware_info, pluginlib::ClassLoader & loader,
std::vector & container)
{
- RCUTILS_LOG_INFO_NAMED(
- "resource_manager", "Loading hardware '%s' ", hardware_info.name.c_str());
- // hardware_plugin_name has to match class name in plugin xml description
- auto interface = std::unique_ptr(
- loader.createUnmanagedInstance(hardware_info.hardware_plugin_name));
- HardwareT hardware(std::move(interface));
- container.emplace_back(std::move(hardware));
- // initialize static data about hardware component to reduce later calls
- HardwareComponentInfo component_info;
- component_info.name = hardware_info.name;
- component_info.type = hardware_info.type;
- component_info.plugin_name = hardware_info.hardware_plugin_name;
- component_info.is_async = hardware_info.is_async;
-
- hardware_info_map_.insert(std::make_pair(component_info.name, component_info));
- hardware_used_by_controllers_.insert(
- std::make_pair(component_info.name, std::vector()));
+ bool is_loaded = false;
+ try
+ {
+ RCUTILS_LOG_INFO_NAMED(
+ "resource_manager", "Loading hardware '%s' ", hardware_info.name.c_str());
+ // hardware_plugin_name has to match class name in plugin xml description
+ auto interface = std::unique_ptr(
+ loader.createUnmanagedInstance(hardware_info.hardware_plugin_name));
+ if (interface)
+ {
+ RCUTILS_LOG_INFO_NAMED(
+ "resource_manager", "Loaded hardware '%s' from plugin '%s'", hardware_info.name.c_str(),
+ hardware_info.hardware_plugin_name.c_str());
+ HardwareT hardware(std::move(interface));
+ container.emplace_back(std::move(hardware));
+ // initialize static data about hardware component to reduce later calls
+ HardwareComponentInfo component_info;
+ component_info.name = hardware_info.name;
+ component_info.type = hardware_info.type;
+ component_info.group = hardware_info.group;
+ component_info.plugin_name = hardware_info.hardware_plugin_name;
+ component_info.is_async = hardware_info.is_async;
+
+ hardware_info_map_.insert(std::make_pair(component_info.name, component_info));
+ hw_group_state_.insert(std::make_pair(component_info.group, return_type::OK));
+ hardware_used_by_controllers_.insert(
+ std::make_pair(component_info.name, std::vector()));
+ is_loaded = true;
+ }
+ else
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Failed to load hardware '%s' from plugin '%s'",
+ hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str());
+ }
+ }
+ catch (const pluginlib::PluginlibException & ex)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Exception while loading hardware: %s", ex.what());
+ }
+ catch (const std::exception & ex)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Exception occurred while loading hardware '%s': %s",
+ hardware_info.name.c_str(), ex.what());
+ }
+ catch (...)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Unknown exception occurred while loading hardware '%s'",
+ hardware_info.name.c_str());
+ }
+ return is_loaded;
}
template
@@ -119,30 +176,62 @@ class ResourceStorage
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Initialize hardware '%s' ", hardware_info.name.c_str());
- const rclcpp_lifecycle::State new_state = hardware.initialize(hardware_info);
-
- bool result = new_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED;
+ bool result = false;
+ try
+ {
+ const rclcpp_lifecycle::State new_state = hardware.initialize(hardware_info);
+ result = new_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED;
- if (result)
+ if (result)
+ {
+ RCUTILS_LOG_INFO_NAMED(
+ "resource_manager", "Successful initialization of hardware '%s'",
+ hardware_info.name.c_str());
+ }
+ else
+ {
+ RCUTILS_LOG_INFO_NAMED(
+ "resource_manager", "Failed to initialize hardware '%s'", hardware_info.name.c_str());
+ }
+ }
+ catch (const std::exception & ex)
{
- RCUTILS_LOG_INFO_NAMED(
- "resource_manager", "Successful initialization of hardware '%s'",
- hardware_info.name.c_str());
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Exception occurred while initializing hardware '%s': %s",
+ hardware_info.name.c_str(), ex.what());
}
- else
+ catch (...)
{
- RCUTILS_LOG_INFO_NAMED(
- "resource_manager", "Failed to initialize hardware '%s'", hardware_info.name.c_str());
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Unknown exception occurred while initializing hardware '%s'",
+ hardware_info.name.c_str());
}
+
return result;
}
template
bool configure_hardware(HardwareT & hardware)
{
- bool result = trigger_and_print_hardware_state_transition(
- std::bind(&HardwareT::configure, &hardware), "configure", hardware.get_name(),
- lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+ bool result = false;
+ try
+ {
+ result = trigger_and_print_hardware_state_transition(
+ std::bind(&HardwareT::configure, &hardware), "configure", hardware.get_name(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+ }
+ catch (const std::exception & ex)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Exception occurred while configuring hardware '%s': %s",
+ hardware.get_name().c_str(), ex.what());
+ }
+ catch (...)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Unknown exception occurred while configuring hardware '%s'",
+ hardware.get_name().c_str());
+ }
if (result)
{
@@ -206,9 +295,15 @@ class ResourceStorage
{
async_component_threads_.emplace(
std::piecewise_construct, std::forward_as_tuple(hardware.get_name()),
- std::forward_as_tuple(&hardware, cm_update_rate_, clock_interface_));
+ std::forward_as_tuple(cm_update_rate_, clock_interface_));
+
+ async_component_threads_.at(hardware.get_name()).register_component(&hardware);
}
}
+ if (!hardware.get_group_name().empty())
+ {
+ hw_group_state_[hardware.get_group_name()] = return_type::OK;
+ }
return result;
}
@@ -267,23 +362,59 @@ class ResourceStorage
template
bool cleanup_hardware(HardwareT & hardware)
{
- bool result = trigger_and_print_hardware_state_transition(
- std::bind(&HardwareT::cleanup, &hardware), "cleanup", hardware.get_name(),
- lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ bool result = false;
+ try
+ {
+ result = trigger_and_print_hardware_state_transition(
+ std::bind(&HardwareT::cleanup, &hardware), "cleanup", hardware.get_name(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+ }
+ catch (const std::exception & ex)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Exception occurred while cleaning up hardware '%s': %s",
+ hardware.get_name().c_str(), ex.what());
+ }
+ catch (...)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Unknown exception occurred while cleaning up hardware '%s'",
+ hardware.get_name().c_str());
+ }
if (result)
{
remove_all_hardware_interfaces_from_available_list(hardware.get_name());
}
+ if (!hardware.get_group_name().empty())
+ {
+ hw_group_state_[hardware.get_group_name()] = return_type::OK;
+ }
return result;
}
template
bool shutdown_hardware(HardwareT & hardware)
{
- bool result = trigger_and_print_hardware_state_transition(
- std::bind(&HardwareT::shutdown, &hardware), "shutdown", hardware.get_name(),
- lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
+ bool result = false;
+ try
+ {
+ result = trigger_and_print_hardware_state_transition(
+ std::bind(&HardwareT::shutdown, &hardware), "shutdown", hardware.get_name(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
+ }
+ catch (const std::exception & ex)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Exception occurred while shutting down hardware '%s': %s",
+ hardware.get_name().c_str(), ex.what());
+ }
+ catch (...)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Unknown exception occurred while shutting down hardware '%s'",
+ hardware.get_name().c_str());
+ }
if (result)
{
@@ -293,6 +424,10 @@ class ResourceStorage
// deimport_non_movement_command_interfaces(hardware);
// deimport_state_interfaces(hardware);
// use remove_command_interfaces(hardware);
+ if (!hardware.get_group_name().empty())
+ {
+ hw_group_state_[hardware.get_group_name()] = return_type::OK;
+ }
}
return result;
}
@@ -300,9 +435,25 @@ class ResourceStorage
template
bool activate_hardware(HardwareT & hardware)
{
- bool result = trigger_and_print_hardware_state_transition(
- std::bind(&HardwareT::activate, &hardware), "activate", hardware.get_name(),
- lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+ bool result = false;
+ try
+ {
+ result = trigger_and_print_hardware_state_transition(
+ std::bind(&HardwareT::activate, &hardware), "activate", hardware.get_name(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+ }
+ catch (const std::exception & ex)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Exception occurred while activating hardware '%s': %s",
+ hardware.get_name().c_str(), ex.what());
+ }
+ catch (...)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Unknown exception occurred while activating hardware '%s'",
+ hardware.get_name().c_str());
+ }
if (result)
{
@@ -319,9 +470,25 @@ class ResourceStorage
template
bool deactivate_hardware(HardwareT & hardware)
{
- bool result = trigger_and_print_hardware_state_transition(
- std::bind(&HardwareT::deactivate, &hardware), "deactivate", hardware.get_name(),
- lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+ bool result = false;
+ try
+ {
+ result = trigger_and_print_hardware_state_transition(
+ std::bind(&HardwareT::deactivate, &hardware), "deactivate", hardware.get_name(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+ }
+ catch (const std::exception & ex)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Exception occurred while deactivating hardware '%s': %s",
+ hardware.get_name().c_str(), ex.what());
+ }
+ catch (...)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Unknown exception occurred while deactivating hardware '%s'",
+ hardware.get_name().c_str());
+ }
if (result)
{
@@ -573,7 +740,10 @@ class ResourceStorage
auto load_and_init_actuators = [&](auto & container)
{
check_for_duplicates(hardware_info);
- load_hardware(hardware_info, actuator_loader_, container);
+ if (!load_hardware(hardware_info, actuator_loader_, container))
+ {
+ return;
+ }
if (initialize_hardware(hardware_info, container.back()))
{
import_state_interfaces(container.back());
@@ -603,7 +773,10 @@ class ResourceStorage
auto load_and_init_sensors = [&](auto & container)
{
check_for_duplicates(hardware_info);
- load_hardware(hardware_info, sensor_loader_, container);
+ if (!load_hardware(hardware_info, sensor_loader_, container))
+ {
+ return;
+ }
if (initialize_hardware(hardware_info, container.back()))
{
import_state_interfaces(container.back());
@@ -632,7 +805,10 @@ class ResourceStorage
auto load_and_init_systems = [&](auto & container)
{
check_for_duplicates(hardware_info);
- load_hardware(hardware_info, system_loader_, container);
+ if (!load_hardware(hardware_info, system_loader_, container))
+ {
+ return;
+ }
if (initialize_hardware(hardware_info, container.back()))
{
import_state_interfaces(container.back());
@@ -746,6 +922,27 @@ class ResourceStorage
}
}
+ /**
+ * Returns the return type of the hardware component group state, if the return type is other
+ * than OK, then updates the return type of the group to the respective one
+ */
+ return_type update_hardware_component_group_state(
+ const std::string & group_name, const return_type & value)
+ {
+ // This is for the components that has no configured group
+ if (group_name.empty())
+ {
+ return value;
+ }
+ // If it is anything other than OK, change the return type of the hardware group state
+ // to the respective return type
+ if (value != return_type::OK)
+ {
+ hw_group_state_.at(group_name) = value;
+ }
+ return hw_group_state_.at(group_name);
+ }
+
// hardware plugins
pluginlib::ClassLoader actuator_loader_;
pluginlib::ClassLoader sensor_loader_;
@@ -760,6 +957,7 @@ class ResourceStorage
std::vector async_systems_;
std::unordered_map hardware_info_map_;
+ std::unordered_map hw_group_state_;
/// Mapping between hardware and controllers that are using it (accessing data from it)
std::unordered_map> hardware_used_by_controllers_;
@@ -1143,24 +1341,6 @@ bool ResourceManager::prepare_command_mode_switch(
return true;
}
- auto interfaces_to_string = [&]()
- {
- std::stringstream ss;
- ss << "Start interfaces: " << std::endl << "[" << std::endl;
- for (const auto & start_if : start_interfaces)
- {
- ss << " " << start_if << std::endl;
- }
- ss << "]" << std::endl;
- ss << "Stop interfaces: " << std::endl << "[" << std::endl;
- for (const auto & stop_if : stop_interfaces)
- {
- ss << " " << stop_if << std::endl;
- }
- ss << "]" << std::endl;
- return ss.str();
- };
-
// Check if interface exists
std::stringstream ss_not_existing;
ss_not_existing << "Not existing: " << std::endl << "[" << std::endl;
@@ -1182,7 +1362,8 @@ bool ResourceManager::prepare_command_mode_switch(
ss_not_existing << "]" << std::endl;
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Not acceptable command interfaces combination: \n%s%s",
- interfaces_to_string().c_str(), ss_not_existing.str().c_str());
+ interfaces_to_string(start_interfaces, stop_interfaces).c_str(),
+ ss_not_existing.str().c_str());
return false;
}
@@ -1207,12 +1388,12 @@ bool ResourceManager::prepare_command_mode_switch(
ss_not_available << "]" << std::endl;
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Not acceptable command interfaces combination: \n%s%s",
- interfaces_to_string().c_str(), ss_not_available.str().c_str());
+ interfaces_to_string(start_interfaces, stop_interfaces).c_str(),
+ ss_not_available.str().c_str());
return false;
}
- auto call_prepare_mode_switch =
- [&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components)
+ auto call_prepare_mode_switch = [&start_interfaces, &stop_interfaces](auto & components)
{
bool ret = true;
for (auto & component : components)
@@ -1221,14 +1402,38 @@ bool ResourceManager::prepare_command_mode_switch(
component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
- if (
- return_type::OK !=
- component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
+ try
+ {
+ if (
+ return_type::OK !=
+ component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager",
+ "Component '%s' did not accept command interfaces combination: \n%s",
+ component.get_name().c_str(),
+ interfaces_to_string(start_interfaces, stop_interfaces).c_str());
+ ret = false;
+ }
+ }
+ catch (const std::exception & e)
{
RCUTILS_LOG_ERROR_NAMED(
"resource_manager",
- "Component '%s' did not accept command interfaces combination: \n%s",
- component.get_name().c_str(), interfaces_to_string().c_str());
+ "Exception occurred while preparing command mode switch for component '%s' for the "
+ "interfaces: \n %s : %s",
+ component.get_name().c_str(),
+ interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what());
+ ret = false;
+ }
+ catch (...)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager",
+ "Unknown exception occurred while preparing command mode switch for component '%s' for "
+ "the interfaces: \n %s",
+ component.get_name().c_str(),
+ interfaces_to_string(start_interfaces, stop_interfaces).c_str());
ret = false;
}
}
@@ -1262,13 +1467,37 @@ bool ResourceManager::perform_command_mode_switch(
component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
- if (
- return_type::OK !=
- component.perform_command_mode_switch(start_interfaces, stop_interfaces))
+ try
+ {
+ if (
+ return_type::OK !=
+ component.perform_command_mode_switch(start_interfaces, stop_interfaces))
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Component '%s' could not perform switch",
+ component.get_name().c_str());
+ ret = false;
+ }
+ }
+ catch (const std::exception & e)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager",
+ "Exception occurred while performing command mode switch for component '%s' for the "
+ "interfaces: \n %s : %s",
+ component.get_name().c_str(),
+ interfaces_to_string(start_interfaces, stop_interfaces).c_str(), e.what());
+ ret = false;
+ }
+ catch (...)
{
RCUTILS_LOG_ERROR_NAMED(
- "resource_manager", "Component '%s' could not perform switch",
- component.get_name().c_str());
+ "resource_manager",
+ "Unknown exception occurred while performing command mode switch for component '%s' "
+ "for "
+ "the interfaces: \n %s",
+ component.get_name().c_str(),
+ interfaces_to_string(start_interfaces, stop_interfaces).c_str());
ret = false;
}
}
@@ -1384,6 +1613,13 @@ return_type ResourceManager::set_component_state(
return result;
}
+void ResourceManager::shutdown_async_components()
+{
+ resource_storage_->async_component_threads_.erase(
+ resource_storage_->async_component_threads_.begin(),
+ resource_storage_->async_component_threads_.end());
+}
+
// CM API: Called in "update"-thread
HardwareReadWriteStatus ResourceManager::read(
const rclcpp::Time & time, const rclcpp::Duration & period)
@@ -1396,7 +1632,28 @@ HardwareReadWriteStatus ResourceManager::read(
{
for (auto & component : components)
{
- auto ret_val = component.read(time, period);
+ auto ret_val = return_type::OK;
+ try
+ {
+ ret_val = component.read(time, period);
+ const auto component_group = component.get_group_name();
+ ret_val =
+ resource_storage_->update_hardware_component_group_state(component_group, ret_val);
+ }
+ catch (const std::exception & e)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Exception thrown durind read of the component '%s': %s",
+ component.get_name().c_str(), e.what());
+ ret_val = return_type::ERROR;
+ }
+ catch (...)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Unknown exception thrown during read of the component '%s'",
+ component.get_name().c_str());
+ ret_val = return_type::ERROR;
+ }
if (ret_val == return_type::ERROR)
{
read_write_status.ok = false;
@@ -1436,7 +1693,28 @@ HardwareReadWriteStatus ResourceManager::write(
{
for (auto & component : components)
{
- auto ret_val = component.write(time, period);
+ auto ret_val = return_type::OK;
+ try
+ {
+ ret_val = component.write(time, period);
+ const auto component_group = component.get_group_name();
+ ret_val =
+ resource_storage_->update_hardware_component_group_state(component_group, ret_val);
+ }
+ catch (const std::exception & e)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Exception thrown during write of the component '%s': %s",
+ component.get_name().c_str(), e.what());
+ ret_val = return_type::ERROR;
+ }
+ catch (...)
+ {
+ RCUTILS_LOG_ERROR_NAMED(
+ "resource_manager", "Unknown exception thrown during write of the component '%s'",
+ component.get_name().c_str());
+ ret_val = return_type::ERROR;
+ }
if (ret_val == return_type::ERROR)
{
read_write_status.ok = false;
diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp
index e758953369..dafe8cb959 100644
--- a/hardware_interface/src/sensor.cpp
+++ b/hardware_interface/src/sensor.cpp
@@ -215,6 +215,8 @@ std::vector> Sensor::export_state_interfaces()
std::string Sensor::get_name() const { return impl_->get_name(); }
+std::string Sensor::get_group_name() const { return impl_->get_group_name(); }
+
const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); }
return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period)
diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp
index a64fb78fba..4b04462101 100644
--- a/hardware_interface/src/system.cpp
+++ b/hardware_interface/src/system.cpp
@@ -257,6 +257,8 @@ return_type System::perform_command_mode_switch(
std::string System::get_name() const { return impl_->get_name(); }
+std::string System::get_group_name() const { return impl_->get_group_name(); }
+
const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_state(); }
return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period)
diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp
index e5b4100e67..fe0a239d9f 100644
--- a/hardware_interface/test/mock_components/test_generic_system.cpp
+++ b/hardware_interface/test/mock_components/test_generic_system.cpp
@@ -96,6 +96,7 @@ class TestGenericSystem : public ::testing::Test
mock_components/GenericSystem
+ Hardware Group
@@ -121,6 +122,7 @@ class TestGenericSystem : public ::testing::Test
mock_components/GenericSystem
+ Hardware Group
@@ -289,6 +291,7 @@ class TestGenericSystem : public ::testing::Test
mock_components/GenericSystem
+ Hardware Group
-3
actual_position
@@ -351,6 +354,7 @@ class TestGenericSystem : public ::testing::Test
mock_components/GenericSystem
+ Hardware Group
2
2
@@ -576,6 +580,70 @@ class TestGenericSystem : public ::testing::Test
)";
+
+ hardware_system_2dof_standard_interfaces_with_same_hardware_group_ =
+ R"(
+
+
+ mock_components/GenericSystem
+ Hardware Group
+
+
+
+
+
+ 3.45
+
+
+
+
+
+
+ mock_components/GenericSystem
+ Hardware Group
+
+
+
+
+
+ 2.78
+
+
+
+
+)";
+
+ hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_ =
+ R"(
+
+
+ mock_components/GenericSystem
+ Hardware Group 1
+
+
+
+
+
+ 3.45
+
+
+
+
+
+
+ mock_components/GenericSystem
+ Hardware Group 2
+
+
+
+
+
+ 2.78
+
+
+
+
+)";
}
std::string hardware_system_2dof_;
@@ -597,6 +665,8 @@ class TestGenericSystem : public ::testing::Test
std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_;
std::string valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_;
std::string disabled_commands_;
+ std::string hardware_system_2dof_standard_interfaces_with_same_hardware_group_;
+ std::string hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_;
};
// Forward declaration
@@ -811,7 +881,7 @@ void generic_system_functional_test(
ASSERT_EQ(0.44, j2v_c.get_value());
// write() does not change values
- rm.write(TIME, PERIOD);
+ ASSERT_TRUE(rm.write(TIME, PERIOD).ok);
ASSERT_EQ(3.45, j1p_s.get_value());
ASSERT_EQ(0.0, j1v_s.get_value());
ASSERT_EQ(2.78, j2p_s.get_value());
@@ -822,7 +892,7 @@ void generic_system_functional_test(
ASSERT_EQ(0.44, j2v_c.get_value());
// read() mirrors commands + offset to states
- rm.read(TIME, PERIOD);
+ ASSERT_TRUE(rm.read(TIME, PERIOD).ok);
ASSERT_EQ(0.11 + offset, j1p_s.get_value());
ASSERT_EQ(0.22, j1v_s.get_value());
ASSERT_EQ(0.33 + offset, j2p_s.get_value());
@@ -854,6 +924,158 @@ void generic_system_functional_test(
status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE);
}
+void generic_system_error_group_test(
+ const std::string & urdf, const std::string component_prefix, bool validate_same_group)
+{
+ TestableResourceManager rm(urdf);
+ const std::string component1 = component_prefix + "1";
+ const std::string component2 = component_prefix + "2";
+ // check is hardware is configured
+ auto status_map = rm.get_components_status();
+ for (auto component : {component1, component2})
+ {
+ EXPECT_EQ(
+ status_map[component].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED);
+ configure_components(rm, {component});
+ status_map = rm.get_components_status();
+ EXPECT_EQ(
+ status_map[component].state.label(), hardware_interface::lifecycle_state_names::INACTIVE);
+ activate_components(rm, {component});
+ status_map = rm.get_components_status();
+ EXPECT_EQ(
+ status_map[component].state.label(), hardware_interface::lifecycle_state_names::ACTIVE);
+ }
+
+ // Check initial values
+ hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position");
+ hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity");
+ hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position");
+ hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface("joint2/velocity");
+ hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position");
+ hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity");
+ hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface("joint2/position");
+ hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity");
+
+ // State interfaces without initial value are set to 0
+ ASSERT_EQ(3.45, j1p_s.get_value());
+ ASSERT_EQ(0.0, j1v_s.get_value());
+ ASSERT_EQ(2.78, j2p_s.get_value());
+ ASSERT_EQ(0.0, j2v_s.get_value());
+ ASSERT_TRUE(std::isnan(j1p_c.get_value()));
+ ASSERT_TRUE(std::isnan(j1v_c.get_value()));
+ ASSERT_TRUE(std::isnan(j2p_c.get_value()));
+ ASSERT_TRUE(std::isnan(j2v_c.get_value()));
+
+ // set some new values in commands
+ j1p_c.set_value(0.11);
+ j1v_c.set_value(0.22);
+ j2p_c.set_value(0.33);
+ j2v_c.set_value(0.44);
+
+ // State values should not be changed
+ ASSERT_EQ(3.45, j1p_s.get_value());
+ ASSERT_EQ(0.0, j1v_s.get_value());
+ ASSERT_EQ(2.78, j2p_s.get_value());
+ ASSERT_EQ(0.0, j2v_s.get_value());
+ ASSERT_EQ(0.11, j1p_c.get_value());
+ ASSERT_EQ(0.22, j1v_c.get_value());
+ ASSERT_EQ(0.33, j2p_c.get_value());
+ ASSERT_EQ(0.44, j2v_c.get_value());
+
+ // write() does not change values
+ ASSERT_TRUE(rm.write(TIME, PERIOD).ok);
+ ASSERT_EQ(3.45, j1p_s.get_value());
+ ASSERT_EQ(0.0, j1v_s.get_value());
+ ASSERT_EQ(2.78, j2p_s.get_value());
+ ASSERT_EQ(0.0, j2v_s.get_value());
+ ASSERT_EQ(0.11, j1p_c.get_value());
+ ASSERT_EQ(0.22, j1v_c.get_value());
+ ASSERT_EQ(0.33, j2p_c.get_value());
+ ASSERT_EQ(0.44, j2v_c.get_value());
+
+ // read() mirrors commands to states
+ ASSERT_TRUE(rm.read(TIME, PERIOD).ok);
+ ASSERT_EQ(0.11, j1p_s.get_value());
+ ASSERT_EQ(0.22, j1v_s.get_value());
+ ASSERT_EQ(0.33, j2p_s.get_value());
+ ASSERT_EQ(0.44, j2v_s.get_value());
+ ASSERT_EQ(0.11, j1p_c.get_value());
+ ASSERT_EQ(0.22, j1v_c.get_value());
+ ASSERT_EQ(0.33, j2p_c.get_value());
+ ASSERT_EQ(0.44, j2v_c.get_value());
+
+ // set some new values in commands
+ j1p_c.set_value(0.55);
+ j1v_c.set_value(0.66);
+ j2p_c.set_value(0.77);
+ j2v_c.set_value(0.88);
+
+ // state values should not be changed
+ ASSERT_EQ(0.11, j1p_s.get_value());
+ ASSERT_EQ(0.22, j1v_s.get_value());
+ ASSERT_EQ(0.33, j2p_s.get_value());
+ ASSERT_EQ(0.44, j2v_s.get_value());
+ ASSERT_EQ(0.55, j1p_c.get_value());
+ ASSERT_EQ(0.66, j1v_c.get_value());
+ ASSERT_EQ(0.77, j2p_c.get_value());
+ ASSERT_EQ(0.88, j2v_c.get_value());
+
+ // Error testing
+ j1p_c.set_value(std::numeric_limits::infinity());
+ j1v_c.set_value(std::numeric_limits::infinity());
+ // read() should now bring error in the first component
+ auto read_result = rm.read(TIME, PERIOD);
+ ASSERT_FALSE(read_result.ok);
+ if (validate_same_group)
+ {
+ // If they belong to the same group, show the error in all hardware components of same group
+ EXPECT_THAT(read_result.failed_hardware_names, ::testing::ElementsAre(component1, component2));
+ }
+ else
+ {
+ // If they don't belong to the same group, show the error in only that hardware component
+ EXPECT_THAT(read_result.failed_hardware_names, ::testing::ElementsAre(component1));
+ }
+
+ // Check initial values
+ ASSERT_FALSE(rm.state_interface_is_available("joint1/position"));
+ ASSERT_FALSE(rm.state_interface_is_available("joint1/velocity"));
+ ASSERT_FALSE(rm.command_interface_is_available("joint1/position"));
+ ASSERT_FALSE(rm.command_interface_is_available("joint1/velocity"));
+
+ if (validate_same_group)
+ {
+ ASSERT_FALSE(rm.state_interface_is_available("joint2/position"));
+ ASSERT_FALSE(rm.state_interface_is_available("joint2/velocity"));
+ ASSERT_FALSE(rm.command_interface_is_available("joint2/position"));
+ ASSERT_FALSE(rm.command_interface_is_available("joint2/velocity"));
+ }
+ else
+ {
+ ASSERT_TRUE(rm.state_interface_is_available("joint2/position"));
+ ASSERT_TRUE(rm.state_interface_is_available("joint2/velocity"));
+ ASSERT_TRUE(rm.command_interface_is_available("joint2/position"));
+ ASSERT_TRUE(rm.command_interface_is_available("joint2/velocity"));
+ }
+
+ // Error should be recoverable only after reactivating the hardware component
+ j1p_c.set_value(0.0);
+ j1v_c.set_value(0.0);
+ ASSERT_FALSE(rm.read(TIME, PERIOD).ok);
+
+ // Now it should be recoverable
+ deactivate_components(rm, {component1});
+ activate_components(rm, {component1});
+ ASSERT_TRUE(rm.read(TIME, PERIOD).ok);
+
+ deactivate_components(rm, {component1, component2});
+ status_map = rm.get_components_status();
+ EXPECT_EQ(
+ status_map[component1].state.label(), hardware_interface::lifecycle_state_names::INACTIVE);
+ EXPECT_EQ(
+ status_map[component2].state.label(), hardware_interface::lifecycle_state_names::INACTIVE);
+}
+
TEST_F(TestGenericSystem, generic_system_2dof_functionality)
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_ +
@@ -862,6 +1084,24 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality)
generic_system_functional_test(urdf, {"MockHardwareSystem"});
}
+TEST_F(TestGenericSystem, generic_system_2dof_error_propagation_different_group)
+{
+ auto urdf = ros2_control_test_assets::urdf_head +
+ hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_ +
+ ros2_control_test_assets::urdf_tail;
+
+ generic_system_error_group_test(urdf, {"MockHardwareSystem"}, false);
+}
+
+TEST_F(TestGenericSystem, generic_system_2dof_error_propagation_same_group)
+{
+ auto urdf = ros2_control_test_assets::urdf_head +
+ hardware_system_2dof_standard_interfaces_with_same_hardware_group_ +
+ ros2_control_test_assets::urdf_tail;
+
+ generic_system_error_group_test(urdf, {"MockHardwareSystem"}, true);
+}
+
TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces)
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_other_interface_ +
diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp
index e5b4991405..fff2e45323 100644
--- a/hardware_interface/test/test_component_parser.cpp
+++ b/hardware_interface/test/test_component_parser.cpp
@@ -32,6 +32,7 @@ class TestComponentParser : public Test
void SetUp() override {}
};
+using hardware_interface::HW_IF_ACCELERATION;
using hardware_interface::HW_IF_EFFORT;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
@@ -112,6 +113,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface)
EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnly");
EXPECT_EQ(hardware_info.type, "system");
+ ASSERT_THAT(hardware_info.group, IsEmpty());
EXPECT_EQ(
hardware_info.hardware_plugin_name,
"ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware");
@@ -137,6 +139,30 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface)
EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1");
ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1));
EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION);
+
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(2));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
+ char joint2_name[] = "joint2";
+ for (const auto & joint : {"joint1", "joint2"})
+ {
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ // position limits will be limited than original range, as their range is limited in the
+ // ros2_control tags
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ if (strcmp(joint, joint2_name) == 0)
+ {
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
+ }
+ }
}
TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface)
@@ -151,6 +177,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface
EXPECT_EQ(hardware_info.name, "RRBotSystemMultiInterface");
EXPECT_EQ(hardware_info.type, "system");
+ ASSERT_THAT(hardware_info.group, IsEmpty());
EXPECT_EQ(
hardware_info.hardware_plugin_name,
"ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware");
@@ -174,6 +201,31 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface
ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1));
ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3));
EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, HW_IF_EFFORT);
+
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(2));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
+ char joint2_name[] = "joint2";
+ for (const auto & joint : {"joint1", "joint2"})
+ {
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ // position limits will be limited than original range, as their range is limited in the
+ // ros2_control tags
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ // effort and velocity limits won't change as they are above the main URDF hard limits
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ if (strcmp(joint, joint2_name) == 0)
+ {
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
+ }
+ }
}
TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sensor)
@@ -188,6 +240,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens
EXPECT_EQ(hardware_info.name, "RRBotSystemWithSensor");
EXPECT_EQ(hardware_info.type, "system");
+ ASSERT_THAT(hardware_info.group, IsEmpty());
EXPECT_EQ(
hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithSensorHardware");
ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
@@ -218,6 +271,30 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens
EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp");
EXPECT_EQ(hardware_info.sensors[0].parameters.at("lower_limits"), "-100");
EXPECT_EQ(hardware_info.sensors[0].parameters.at("upper_limits"), "100");
+
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(2));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
+ char joint2_name[] = "joint2";
+ for (const auto & joint : {"joint1", "joint2"})
+ {
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ // position limits will be limited than original range, as their range is limited in the
+ // ros2_control tags
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ if (strcmp(joint, joint2_name) == 0)
+ {
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
+ }
+ }
}
TEST_F(
@@ -334,6 +411,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte
EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnlyWithExternalSensor");
EXPECT_EQ(hardware_info.type, "system");
+ ASSERT_THAT(hardware_info.group, IsEmpty());
EXPECT_EQ(
hardware_info.hardware_plugin_name,
"ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware");
@@ -350,6 +428,30 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte
ASSERT_THAT(hardware_info.sensors, SizeIs(0));
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(2));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
+ char joint2_name[] = "joint2";
+ for (const auto & joint : {"joint1", "joint2"})
+ {
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ // position limits will be limited than original range, as their range is limited in the
+ // ros2_control tags
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ if (strcmp(joint, joint2_name) == 0)
+ {
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
+ }
+ }
+
hardware_info = control_hardware.at(1);
EXPECT_EQ(hardware_info.name, "RRBotForceTorqueSensor2D");
@@ -361,6 +463,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte
EXPECT_EQ(hardware_info.sensors[0].name, "tcp_fts_sensor");
EXPECT_EQ(hardware_info.sensors[0].type, "sensor");
EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp");
+ ASSERT_THAT(hardware_info.limits, SizeIs(0));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
}
TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot)
@@ -374,6 +478,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
auto hardware_info = control_hardware.at(0);
EXPECT_EQ(hardware_info.name, "RRBotModularJoint1");
+ EXPECT_EQ(hardware_info.group, "Hardware Group");
EXPECT_EQ(hardware_info.type, "actuator");
EXPECT_EQ(
hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionActuatorHardware");
@@ -383,10 +488,26 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
ASSERT_THAT(hardware_info.joints, SizeIs(1));
EXPECT_EQ(hardware_info.joints[0].name, "joint1");
EXPECT_EQ(hardware_info.joints[0].type, "joint");
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(1));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
+ for (const auto & joint : {"joint1"})
+ {
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ // position limits will be limited than original range, as their range is limited in the
+ // ros2_control tags
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ }
hardware_info = control_hardware.at(1);
EXPECT_EQ(hardware_info.name, "RRBotModularJoint2");
+ EXPECT_EQ(hardware_info.group, "Hardware Group");
EXPECT_EQ(hardware_info.type, "actuator");
EXPECT_EQ(
hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionActuatorHardware");
@@ -396,6 +517,29 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
ASSERT_THAT(hardware_info.joints, SizeIs(1));
EXPECT_EQ(hardware_info.joints[0].name, "joint2");
EXPECT_EQ(hardware_info.joints[0].type, "joint");
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(1));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
+ char joint2_name[] = "joint2";
+ for (const auto & joint : {"joint2"})
+ {
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ // position limits will be limited than original range, as their range is limited in the
+ // ros2_control tags
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ if (strcmp(joint, joint2_name) == 0)
+ {
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
+ }
+ }
}
TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot_with_sensors)
@@ -409,6 +553,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
auto hardware_info = control_hardware.at(0);
EXPECT_EQ(hardware_info.name, "RRBotModularJoint1");
+ EXPECT_EQ(hardware_info.group, "Hardware Group 1");
EXPECT_EQ(hardware_info.type, "actuator");
EXPECT_EQ(
hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/VelocityActuatorHardware");
@@ -430,10 +575,25 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
hardware_info.transmissions[0].joints[0].mechanical_reduction, DoubleNear(1024.0 / M_PI, 0.01));
ASSERT_THAT(hardware_info.transmissions[0].actuators, SizeIs(1));
EXPECT_THAT(hardware_info.transmissions[0].actuators[0].name, "actuator1");
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(1));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
+ for (const auto & joint : {"joint1"})
+ {
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ // effort and velocity limits won't change as they are above the main URDF hard limits
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ }
hardware_info = control_hardware.at(1);
EXPECT_EQ(hardware_info.name, "RRBotModularJoint2");
+ EXPECT_EQ(hardware_info.group, "Hardware Group 2");
EXPECT_EQ(hardware_info.type, "actuator");
EXPECT_EQ(
hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/VelocityActuatorHardware");
@@ -447,10 +607,33 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY);
EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1");
EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1");
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(1));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
+ char joint2_name[] = "joint2";
+ for (const auto & joint : {"joint2"})
+ {
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ // effort and velocity limits won't change as they are above the main URDF hard limits
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ if (strcmp(joint, joint2_name) == 0)
+ {
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
+ }
+ }
hardware_info = control_hardware.at(2);
EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint1");
+ EXPECT_EQ(hardware_info.group, "Hardware Group 1");
EXPECT_EQ(hardware_info.type, "sensor");
EXPECT_EQ(
hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionSensorHardware");
@@ -464,10 +647,25 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty());
ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1));
EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION);
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(1));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
+ for (const auto & joint : {"joint1"})
+ {
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ // effort and velocity limits won't change as they are above the main URDF hard limits
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ }
hardware_info = control_hardware.at(3);
EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint2");
+ EXPECT_EQ(hardware_info.group, "Hardware Group 2");
EXPECT_EQ(hardware_info.type, "sensor");
EXPECT_EQ(
hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionSensorHardware");
@@ -481,6 +679,27 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty());
ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1));
EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION);
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(1));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
+ for (const auto & joint : {"joint2"})
+ {
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ // effort and velocity limits won't change as they are above the main URDF hard limits
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ if (strcmp(joint, joint2_name) == 0)
+ {
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
+ }
+ }
}
TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_transmission)
@@ -495,6 +714,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_tr
EXPECT_EQ(hardware_info.name, "RRBotModularWrist");
EXPECT_EQ(hardware_info.type, "system");
+ ASSERT_THAT(hardware_info.group, IsEmpty());
EXPECT_EQ(
hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/ActuatorHardwareMultiDOF");
ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
@@ -537,6 +757,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only)
EXPECT_EQ(hardware_info.name, "CameraWithIMU");
EXPECT_EQ(hardware_info.type, "sensor");
+ ASSERT_THAT(hardware_info.group, IsEmpty());
EXPECT_EQ(hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/CameraWithIMUSensor");
ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1));
EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2");
@@ -553,6 +774,9 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only)
EXPECT_EQ(hardware_info.sensors[1].type, "sensor");
ASSERT_THAT(hardware_info.sensors[1].state_interfaces, SizeIs(1));
EXPECT_EQ(hardware_info.sensors[1].state_interfaces[0].name, "image");
+ // There will be no limits as the ros2_control tag has only sensor info
+ ASSERT_THAT(hardware_info.limits, SizeIs(0));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
}
TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only)
@@ -602,6 +826,18 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only)
EXPECT_THAT(actuator.offset, DoubleEq(0.0));
ASSERT_THAT(transmission.parameters, SizeIs(1));
EXPECT_EQ(transmission.parameters.at("additional_special_parameter"), "1337");
+
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(1));
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(M_PI, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-M_PI, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits);
+ // effort and velocity limits won't change as they are above the main URDF hard limits
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
}
TEST_F(TestComponentParser, successfully_parse_locale_independent_double)
@@ -670,6 +906,29 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio
EXPECT_EQ(hardware_info.gpios[1].command_interfaces[0].name, "vacuum");
EXPECT_THAT(hardware_info.transmissions, IsEmpty());
+
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(2));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
+ char joint2_name[] = "joint2";
+ for (const auto & joint : {"joint1", "joint2"})
+ {
+ // Position limits are limited in the ros2_control tag
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ if (strcmp(joint, joint2_name) == 0)
+ {
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
+ }
+ }
}
TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_data_type)
@@ -718,6 +977,366 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d
EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1);
}
+TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_interfaces)
+{
+ std::string urdf_to_test =
+ std::string(ros2_control_test_assets::urdf_head) +
+ ros2_control_test_assets::
+ valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits +
+ ros2_control_test_assets::urdf_tail;
+ const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
+ ASSERT_THAT(control_hardware, SizeIs(1));
+ auto hardware_info = control_hardware.front();
+
+ EXPECT_EQ(hardware_info.name, "RRBotSystemWithGPIO");
+ EXPECT_EQ(hardware_info.type, "system");
+ EXPECT_EQ(
+ hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware");
+
+ ASSERT_THAT(hardware_info.joints, SizeIs(3));
+
+ EXPECT_EQ(hardware_info.joints[0].name, "joint1");
+ EXPECT_EQ(hardware_info.joints[0].type, "joint");
+ EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(5));
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].size, 1);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].size, 1);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].name, HW_IF_ACCELERATION);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].size, 1);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].name, "jerk");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].size, 1);
+ EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1));
+ EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION);
+ EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].size, 1);
+
+ EXPECT_EQ(hardware_info.joints[1].name, "joint2");
+ EXPECT_EQ(hardware_info.joints[1].type, "joint");
+ EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(5));
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].size, 1);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].name, HW_IF_VELOCITY);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].size, 1);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].name, HW_IF_EFFORT);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].size, 1);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].name, HW_IF_ACCELERATION);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].size, 1);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].name, "jerk");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].size, 1);
+ EXPECT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1));
+ EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION);
+ EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].size, 1);
+
+ EXPECT_EQ(hardware_info.joints[2].name, "joint3");
+ EXPECT_EQ(hardware_info.joints[2].type, "joint");
+ EXPECT_THAT(hardware_info.joints[2].command_interfaces, SizeIs(1));
+ EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].name, HW_IF_ACCELERATION);
+ EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].size, 1);
+
+ ASSERT_THAT(hardware_info.gpios, SizeIs(1));
+
+ EXPECT_EQ(hardware_info.gpios[0].name, "flange_IOS");
+ EXPECT_EQ(hardware_info.gpios[0].type, "gpio");
+ EXPECT_THAT(hardware_info.gpios[0].command_interfaces, SizeIs(1));
+ EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].name, "digital_output");
+ EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].data_type, "bool");
+ EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].size, 2);
+ EXPECT_THAT(hardware_info.gpios[0].state_interfaces, SizeIs(2));
+ EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].name, "analog_input");
+ EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].data_type, "double");
+ EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].size, 3);
+ EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].name, "image");
+ EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].data_type, "cv::Mat");
+ EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1);
+
+ EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits);
+ EXPECT_THAT(
+ hardware_info.limits.at("joint1").max_position,
+ DoubleNear(std::numeric_limits::max(), 1e-5));
+ EXPECT_THAT(
+ hardware_info.limits.at("joint1").min_position,
+ DoubleNear(-std::numeric_limits::max(), 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5));
+
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits);
+ EXPECT_THAT(
+ hardware_info.limits.at("joint2").max_position,
+ DoubleNear(std::numeric_limits::max(), 1e-5));
+ EXPECT_THAT(
+ hardware_info.limits.at("joint2").min_position,
+ DoubleNear(-std::numeric_limits::max(), 1e-5));
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at("joint2").max_position, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at("joint2").min_position, DoubleNear(-1.5, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at("joint2").k_position, DoubleNear(10.0, 1e-5));
+ EXPECT_THAT(hardware_info.soft_limits.at("joint2").k_velocity, DoubleNear(20.0, 1e-5));
+
+ EXPECT_TRUE(hardware_info.limits.at("joint3").has_position_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint3").min_position, DoubleNear(-M_PI, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint3").max_position, DoubleNear(M_PI, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at("joint3").has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint3").has_effort_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint3").has_acceleration_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint3").has_deceleration_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint3").has_jerk_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint3").max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint3").max_effort, DoubleNear(0.1, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint3").max_acceleration, DoubleNear(1.0, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5));
+}
+
+TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_unavailable_interfaces)
+{
+ std::string urdf_to_test =
+ std::string(ros2_control_test_assets::urdf_head) +
+ ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_unavailable_interfaces +
+ ros2_control_test_assets::urdf_tail;
+ const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
+ ASSERT_THAT(control_hardware, SizeIs(1));
+ auto hardware_info = control_hardware.front();
+
+ EXPECT_EQ(hardware_info.name, "RRBotSystemWithGPIO");
+ EXPECT_EQ(hardware_info.type, "system");
+ EXPECT_EQ(
+ hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware");
+
+ ASSERT_THAT(hardware_info.joints, SizeIs(3));
+
+ EXPECT_EQ(hardware_info.joints[0].name, "joint1");
+ EXPECT_EQ(hardware_info.joints[0].type, "joint");
+ EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(5));
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].size, 1);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].size, 1);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].name, HW_IF_ACCELERATION);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].size, 1);
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].name, "jerk");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].size, 1);
+ EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1));
+ EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION);
+ EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].size, 1);
+
+ EXPECT_EQ(hardware_info.joints[1].name, "joint2");
+ EXPECT_EQ(hardware_info.joints[1].type, "joint");
+ EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(5));
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].size, 1);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].name, HW_IF_VELOCITY);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].size, 1);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].name, HW_IF_EFFORT);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].size, 1);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].name, HW_IF_ACCELERATION);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].size, 1);
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].name, "jerk");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].size, 1);
+ EXPECT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1));
+ EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION);
+ EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].size, 1);
+
+ EXPECT_EQ(hardware_info.joints[2].name, "joint3");
+ EXPECT_EQ(hardware_info.joints[2].type, "joint");
+ EXPECT_THAT(hardware_info.joints[2].command_interfaces, SizeIs(2));
+ EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].name, HW_IF_ACCELERATION);
+ EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].data_type, "double");
+ EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].size, 1);
+
+ ASSERT_THAT(hardware_info.gpios, SizeIs(0));
+
+ EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits);
+ EXPECT_THAT(
+ hardware_info.limits.at("joint1").max_position,
+ DoubleNear(std::numeric_limits::max(), 1e-5));
+ EXPECT_THAT(
+ hardware_info.limits.at("joint1").min_position,
+ DoubleNear(-std::numeric_limits::max(), 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5));
+
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits);
+ EXPECT_THAT(
+ hardware_info.limits.at("joint2").max_position,
+ DoubleNear(std::numeric_limits::max(), 1e-5));
+ EXPECT_THAT(
+ hardware_info.limits.at("joint2").min_position,
+ DoubleNear(-std::numeric_limits::max(), 1e-5));
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5));
+
+ EXPECT_TRUE(hardware_info.limits.at("joint3").has_position_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint3").min_position, DoubleNear(-M_PI, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint3").max_position, DoubleNear(M_PI, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at("joint3").has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint3").has_effort_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint3").has_acceleration_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint3").has_deceleration_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint3").has_jerk_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint3").max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint3").max_effort, DoubleNear(0.1, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint3").max_acceleration, DoubleNear(1.0, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5));
+}
+
+TEST_F(TestComponentParser, throw_on_parse_invalid_urdf_system_missing_limits)
+{
+ std::string urdf_to_test =
+ std::string(ros2_control_test_assets::urdf_head_revolute_missing_limits) +
+ ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface +
+ ros2_control_test_assets::urdf_tail;
+ EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
+
+ urdf_to_test = std::string(ros2_control_test_assets::urdf_head_prismatic_missing_limits) +
+ ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface +
+ ros2_control_test_assets::urdf_tail;
+ EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
+}
+
+TEST_F(TestComponentParser, throw_on_parse_urdf_system_with_command_fixed_joint)
+{
+ std::string urdf_to_test =
+ std::string(ros2_control_test_assets::urdf_head) +
+ ros2_control_test_assets::invalid_urdf_ros2_control_system_with_command_fixed_joint +
+ ros2_control_test_assets::urdf_tail;
+ EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
+}
+
+TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_missing_limits)
+{
+ std::string urdf_to_test =
+ std::string(ros2_control_test_assets::urdf_head_continuous_missing_limits) +
+ ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_all_interfaces +
+ ros2_control_test_assets::urdf_tail;
+ EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test));
+
+ const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
+ ASSERT_THAT(control_hardware, SizeIs(1));
+ auto hardware_info = control_hardware.front();
+
+ ASSERT_GT(hardware_info.limits.count("joint1"), 0);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(1.0, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5))
+ << "velocity constraint from ros2_control_tag";
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.2, 1e-5))
+ << "effort constraint from ros2_control_tag";
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5));
+
+ ASSERT_GT(hardware_info.limits.count("joint2"), 0);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits);
+}
+
+TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_with_limits)
+{
+ std::string urdf_to_test =
+ std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) +
+ ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_all_interfaces +
+ ros2_control_test_assets::urdf_tail;
+ EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test));
+
+ const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
+ ASSERT_THAT(control_hardware, SizeIs(1));
+ auto hardware_info = control_hardware.front();
+
+ ASSERT_GT(hardware_info.limits.count("joint1"), 0);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-1.0, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(1.0, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5))
+ << "velocity URDF constraint overridden by ros2_control tag";
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5))
+ << "effort constraint from URDF";
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5));
+
+ ASSERT_GT(hardware_info.limits.count("joint2"), 0);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint2").has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at("joint2").has_effort_limits);
+ EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5))
+ << "velocity constraint from URDF";
+ EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5))
+ << "effort constraint from URDF";
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits);
+ EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits);
+}
+
TEST_F(TestComponentParser, successfully_parse_parameter_empty)
{
const std::string urdf_to_test =
@@ -742,6 +1361,20 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty)
EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "");
EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2");
+
+ // Verify limits parsed from the URDF
+ ASSERT_THAT(hardware_info.limits, SizeIs(1));
+ ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
+ for (const auto & joint : {"joint1"})
+ {
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5));
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits);
+ EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
+ EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
+ EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
+ }
}
TEST_F(TestComponentParser, negative_size_throws_error)
diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst
index 58f6edbcdb..66d8794675 100644
--- a/hardware_interface_testing/CHANGELOG.rst
+++ b/hardware_interface_testing/CHANGELOG.rst
@@ -2,6 +2,17 @@
Changelog for package hardware_interface_testing
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.11.0 (2024-05-14)
+-------------------
+
+4.10.0 (2024-05-08)
+-------------------
+
+4.9.0 (2024-04-30)
+------------------
+* Component parser: Get mimic information from URDF (`#1256 `_)
+* Contributors: Christoph Fröhlich
+
4.8.0 (2024-03-27)
------------------
diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml
index 49968cfd56..b63e76ec6d 100644
--- a/hardware_interface_testing/package.xml
+++ b/hardware_interface_testing/package.xml
@@ -1,7 +1,7 @@
hardware_interface_testing
- 4.8.0
+ 4.11.0
ros2_control hardware interface testing
Bence Magyar
Denis Štogl
diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst
index e318e1d3c0..12cfce3bd6 100644
--- a/joint_limits/CHANGELOG.rst
+++ b/joint_limits/CHANGELOG.rst
@@ -2,6 +2,18 @@
Changelog for package joint_limits
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+4.11.0 (2024-05-14)
+-------------------
+* Fix dependencies for source build (`#1533 `_)
+* Add find_package for ament_cmake_gen_version_h (`#1534 `_)
+* Contributors: Christoph Fröhlich
+
+4.10.0 (2024-05-08)
+-------------------
+
+4.9.0 (2024-04-30)
+------------------
+
4.8.0 (2024-03-27)
------------------
* generate version.h file per package using the ament_generate_version_header (`#1449 `_)
diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt
index 1139a5248e..8be804fa23 100644
--- a/joint_limits/CMakeLists.txt
+++ b/joint_limits/CMakeLists.txt
@@ -7,12 +7,17 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
endif()
set(THIS_PACKAGE_INCLUDE_DEPENDS
+ pluginlib
+ realtime_tools
rclcpp
rclcpp_lifecycle
+ trajectory_msgs
urdf
)
find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+find_package(ament_cmake_gen_version_h REQUIRED)
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach()
@@ -25,10 +30,42 @@ target_include_directories(joint_limits INTERFACE
)
ament_target_dependencies(joint_limits INTERFACE ${THIS_PACKAGE_INCLUDE_DEPENDS})
+add_library(joint_limiter_interface SHARED
+ src/joint_limiter_interface.cpp
+)
+target_compile_features(joint_limiter_interface PUBLIC cxx_std_17)
+target_include_directories(joint_limiter_interface PUBLIC
+ $
+ $
+)
+ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(joint_limiter_interface PRIVATE "JOINT_LIMITS_BUILDING_DLL")
+
+
+add_library(joint_saturation_limiter SHARED
+ src/joint_saturation_limiter.cpp
+)
+target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17)
+target_include_directories(joint_saturation_limiter PUBLIC
+ $
+ $
+)
+ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(joint_saturation_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml)
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
+ find_package(ament_cmake_gmock REQUIRED)
+ find_package(generate_parameter_library REQUIRED)
find_package(launch_testing_ament_cmake REQUIRED)
+ find_package(pluginlib REQUIRED)
+ find_package(rclcpp REQUIRED)
ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp)
target_link_libraries(joint_limits_rosparam_test joint_limits)
@@ -42,16 +79,31 @@ if(BUILD_TESTING)
DESTINATION lib/joint_limits
)
install(
- FILES test/joint_limits_rosparam.yaml
+ FILES test/joint_limits_rosparam.yaml test/joint_saturation_limiter_param.yaml
DESTINATION share/joint_limits/test
)
+
+ add_rostest_with_parameters_gmock(test_joint_saturation_limiter test/test_joint_saturation_limiter.cpp
+ ${CMAKE_CURRENT_SOURCE_DIR}/test/joint_saturation_limiter_param.yaml
+ )
+ target_include_directories(test_joint_saturation_limiter PRIVATE include)
+ target_link_libraries(test_joint_saturation_limiter joint_limiter_interface)
+ ament_target_dependencies(
+ test_joint_saturation_limiter
+ pluginlib
+ rclcpp
+ )
+
endif()
install(
DIRECTORY include/
DESTINATION include/joint_limits
)
-install(TARGETS joint_limits
+install(TARGETS
+ joint_limits
+ joint_limiter_interface
+ joint_saturation_limiter
EXPORT export_joint_limits
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp
new file mode 100644
index 0000000000..efaf529724
--- /dev/null
+++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp
@@ -0,0 +1,250 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \author Denis Stogl
+
+#ifndef JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_
+#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_
+
+#include
+#include
+
+#include "joint_limits/joint_limits.hpp"
+#include "joint_limits/joint_limits_rosparam.hpp"
+#include "joint_limits/visibility_control.h"
+#include "rclcpp/node.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+
+namespace joint_limits
+{
+using JointLimitsStateDataType = trajectory_msgs::msg::JointTrajectoryPoint;
+
+template
+class JointLimiterInterface
+{
+public:
+ JOINT_LIMITS_PUBLIC JointLimiterInterface() = default;
+
+ JOINT_LIMITS_PUBLIC virtual ~JointLimiterInterface() = default;
+
+ /// Initialization of every JointLimiter.
+ /**
+ * Initialization of JointLimiter for defined joints with their names.
+ * Robot description topic provides a topic name where URDF of the robot can be found.
+ * This is needed to use joint limits from URDF (not implemented yet!).
+ * Override this method only if initialization and reading joint limits should be adapted.
+ * Otherwise, initialize your custom limiter in `on_limit` method.
+ *
+ * \param[in] joint_names names of joints where limits should be applied.
+ * \param[in] param_itf node parameters interface object to access parameters.
+ * \param[in] logging_itf node logging interface to log if error happens.
+ * \param[in] robot_description_topic string of a topic where robot description is accessible.
+ */
+ JOINT_LIMITS_PUBLIC virtual bool init(
+ const std::vector & joint_names,
+ const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
+ const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
+ const std::string & robot_description_topic = "/robot_description")
+ {
+ number_of_joints_ = joint_names.size();
+ joint_names_ = joint_names;
+ joint_limits_.resize(number_of_joints_);
+ node_param_itf_ = param_itf;
+ node_logging_itf_ = logging_itf;
+
+ bool result = true;
+
+ // TODO(destogl): get limits from URDF
+
+ // Initialize and get joint limits from parameter server
+ for (size_t i = 0; i < number_of_joints_; ++i)
+ {
+ if (!declare_parameters(joint_names[i], node_param_itf_, node_logging_itf_))
+ {
+ RCLCPP_ERROR(
+ node_logging_itf_->get_logger(),
+ "JointLimiter: Joint '%s': parameter declaration has failed", joint_names[i].c_str());
+ result = false;
+ break;
+ }
+ if (!get_joint_limits(joint_names[i], node_param_itf_, node_logging_itf_, joint_limits_[i]))
+ {
+ RCLCPP_ERROR(
+ node_logging_itf_->get_logger(),
+ "JointLimiter: Joint '%s': getting parameters has failed", joint_names[i].c_str());
+ result = false;
+ break;
+ }
+ RCLCPP_INFO(
+ node_logging_itf_->get_logger(), "Limits for joint %zu (%s) are:\n%s", i,
+ joint_names[i].c_str(), joint_limits_[i].to_string().c_str());
+ }
+ updated_limits_.writeFromNonRT(joint_limits_);
+
+ auto on_parameter_event_callback = [this](const std::vector & parameters)
+ {
+ rcl_interfaces::msg::SetParametersResult set_parameters_result;
+ set_parameters_result.successful = true;
+
+ std::vector updated_joint_limits = joint_limits_;
+ bool changed = false;
+
+ for (size_t i = 0; i < number_of_joints_; ++i)
+ {
+ changed |= joint_limits::check_for_limits_update(
+ joint_names_[i], parameters, node_logging_itf_, updated_joint_limits[i]);
+ }
+
+ if (changed)
+ {
+ updated_limits_.writeFromNonRT(updated_joint_limits);
+ RCLCPP_INFO(node_logging_itf_->get_logger(), "Limits are dynamically updated!");
+ }
+
+ return set_parameters_result;
+ };
+
+ parameter_callback_ =
+ node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback);
+
+ if (result)
+ {
+ result = on_init();
+ }
+
+ (void)robot_description_topic; // avoid linters output
+
+ return result;
+ }
+
+ /**
+ * Wrapper init method that accepts pointer to the Node.
+ * For details see other init method.
+ */
+ JOINT_LIMITS_PUBLIC virtual bool init(
+ const std::vector & joint_names, const rclcpp::Node::SharedPtr & node,
+ const std::string & robot_description_topic = "/robot_description")
+ {
+ return init(
+ joint_names, node->get_node_parameters_interface(), node->get_node_logging_interface(),
+ robot_description_topic);
+ }
+
+ /**
+ * Wrapper init method that accepts pointer to the LifecycleNode.
+ * For details see other init method.
+ */
+ JOINT_LIMITS_PUBLIC virtual bool init(
+ const std::vector & joint_names,
+ const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node,
+ const std::string & robot_description_topic = "/robot_description")
+ {
+ return init(
+ joint_names, lifecycle_node->get_node_parameters_interface(),
+ lifecycle_node->get_node_logging_interface(), robot_description_topic);
+ }
+
+ JOINT_LIMITS_PUBLIC virtual bool configure(const JointLimitsStateDataType & current_joint_states)
+ {
+ return on_configure(current_joint_states);
+ }
+
+ /** \brief Enforce joint limits to desired joint state for multiple physical quantities.
+ *
+ * Generic enforce method that calls implementation-specific `on_enforce` method.
+ *
+ * \param[in] current_joint_states current joint states a robot is in.
+ * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits.
+ * \param[in] dt time delta to calculate missing integrals and derivation in joint limits.
+ * \returns true if limits are enforced, otherwise false.
+ */
+ JOINT_LIMITS_PUBLIC virtual bool enforce(
+ JointLimitsStateDataType & current_joint_states,
+ JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt)
+ {
+ joint_limits_ = *(updated_limits_.readFromRT());
+ return on_enforce(current_joint_states, desired_joint_states, dt);
+ }
+
+ /** \brief Enforce joint limits to desired joint state for single physical quantity.
+ *
+ * Generic enforce method that calls implementation-specific `on_enforce` method.
+ *
+ * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits.
+ * \returns true if limits are enforced, otherwise false.
+ */
+ JOINT_LIMITS_PUBLIC virtual bool enforce(std::vector & desired_joint_states)
+ {
+ joint_limits_ = *(updated_limits_.readFromRT());
+ return on_enforce(desired_joint_states);
+ }
+
+protected:
+ /** \brief Method is realized by an implementation.
+ *
+ * Implementation-specific initialization of limiter's internal states and libraries.
+ * \returns true if initialization was successful, otherwise false.
+ */
+ JOINT_LIMITS_PUBLIC virtual bool on_init() = 0;
+
+ /** \brief Method is realized by an implementation.
+ *
+ * Implementation-specific configuration of limiter's internal states and libraries.
+ * \returns true if initialization was successful, otherwise false.
+ */
+ JOINT_LIMITS_PUBLIC virtual bool on_configure(
+ const JointLimitsStateDataType & current_joint_states) = 0;
+
+ /** \brief Method is realized by an implementation.
+ *
+ * Filter-specific implementation of the joint limits enforce algorithm for multiple dependent
+ * physical quantities.
+ *
+ * \param[in] current_joint_states current joint states a robot is in.
+ * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits.
+ * \param[in] dt time delta to calculate missing integrals and derivation in joint limits.
+ * \returns true if limits are enforced, otherwise false.
+ */
+ JOINT_LIMITS_PUBLIC virtual bool on_enforce(
+ JointLimitsStateDataType & current_joint_states,
+ JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0;
+
+ /** \brief Method is realized by an implementation.
+ *
+ * Filter-specific implementation of the joint limits enforce algorithm for single physical
+ * quantity.
+ * This method might use "effort" limits since they are often used as wild-card.
+ * Check the documentation of the exact implementation for more details.
+ *
+ * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits.
+ * \returns true if limits are enforced, otherwise false.
+ */
+ JOINT_LIMITS_PUBLIC virtual bool on_enforce(std::vector & desired_joint_states) = 0;
+
+ size_t number_of_joints_;
+ std::vector joint_names_;
+ std::vector joint_limits_;
+ rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_itf_;
+ rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_itf_;
+
+private:
+ rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_;
+ realtime_tools::RealtimeBuffer> updated_limits_;
+};
+
+} // namespace joint_limits
+
+#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_
diff --git a/joint_limits/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp
index f9944a85b1..809bfd777b 100644
--- a/joint_limits/include/joint_limits/joint_limits.hpp
+++ b/joint_limits/include/joint_limits/joint_limits.hpp
@@ -128,11 +128,14 @@ struct SoftJointLimits
{
std::stringstream ss_output;
- ss_output << " soft position limits: " << "[" << min_position << ", " << max_position << "]\n";
+ ss_output << " soft position limits: "
+ << "[" << min_position << ", " << max_position << "]\n";
- ss_output << " k-position: " << "[" << k_position << "]\n";
+ ss_output << " k-position: "
+ << "[" << k_position << "]\n";
- ss_output << " k-velocity: " << "[" << k_velocity << "]\n";
+ ss_output << " k-velocity: "
+ << "[" << k_velocity << "]\n";
return ss_output.str();
}
diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp
index 2f32d49271..b23607f53d 100644
--- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp
+++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp
@@ -19,6 +19,7 @@
#include
#include
+#include
#include "joint_limits/joint_limits.hpp"
#include "rclcpp/rclcpp.hpp"
@@ -96,8 +97,6 @@ inline bool declare_parameters(
auto_declare(
param_itf, param_base_name + ".max_position", std::numeric_limits::quiet_NaN());
auto_declare(param_itf, param_base_name + ".has_velocity_limits", false);
- auto_declare(
- param_itf, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN());
auto_declare(
param_itf, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN());
auto_declare(param_itf, param_base_name + ".has_acceleration_limits", false);
@@ -237,7 +236,6 @@ inline bool get_joint_limits(
!param_itf->has_parameter(param_base_name + ".min_position") &&
!param_itf->has_parameter(param_base_name + ".max_position") &&
!param_itf->has_parameter(param_base_name + ".has_velocity_limits") &&
- !param_itf->has_parameter(param_base_name + ".min_velocity") &&
!param_itf->has_parameter(param_base_name + ".max_velocity") &&
!param_itf->has_parameter(param_base_name + ".has_acceleration_limits") &&
!param_itf->has_parameter(param_base_name + ".max_acceleration") &&
@@ -247,12 +245,7 @@ inline bool get_joint_limits(
!param_itf->has_parameter(param_base_name + ".max_jerk") &&
!param_itf->has_parameter(param_base_name + ".has_effort_limits") &&
!param_itf->has_parameter(param_base_name + ".max_effort") &&
- !param_itf->has_parameter(param_base_name + ".angle_wraparound") &&
- !param_itf->has_parameter(param_base_name + ".has_soft_limits") &&
- !param_itf->has_parameter(param_base_name + ".k_position") &&
- !param_itf->has_parameter(param_base_name + ".k_velocity") &&
- !param_itf->has_parameter(param_base_name + ".soft_lower_limit") &&
- !param_itf->has_parameter(param_base_name + ".soft_upper_limit"))
+ !param_itf->has_parameter(param_base_name + ".angle_wraparound"))
{
RCLCPP_ERROR(
logging_itf->get_logger(),
@@ -421,6 +414,218 @@ inline bool get_joint_limits(
lifecycle_node->get_node_logging_interface(), limits);
}
+/**
+ * Check if any of updated parameters are related to JointLimits.
+ *
+ * This method is intended to be used in the parameters update callback.
+ * It is recommended that it's result is temporarily stored and synchronized with the JointLimits
+ * structure in the main loop.
+ *
+ * \param[in] joint_name Name of the joint whose limits should be checked.
+ * \param[in] parameters List of the parameters that should be checked if they belong to this
+ * structure and that are used for updating the internal values.
+ * \param[in] logging_itf node logging interface to provide log errors.
+ * \param[out] updated_limits structure with updated JointLimits. It is recommended that the
+ * currently used limits are stored into this structure before calling this method.
+ */
+inline bool check_for_limits_update(
+ const std::string & joint_name, const std::vector & parameters,
+ const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
+ JointLimits & updated_limits)
+{
+ const std::string param_base_name = "joint_limits." + joint_name;
+ bool changed = false;
+
+ // update first numerical values to make later checks for "has" limits members
+ for (auto & parameter : parameters)
+ {
+ const std::string param_name = parameter.get_name();
+ try
+ {
+ if (param_name == param_base_name + ".min_position")
+ {
+ changed = updated_limits.min_position != parameter.get_value();
+ updated_limits.min_position = parameter.get_value();
+ }
+ else if (param_name == param_base_name + ".max_position")
+ {
+ changed = updated_limits.max_position != parameter.get_value();
+ updated_limits.max_position = parameter.get_value();
+ }
+ else if (param_name == param_base_name + ".max_velocity")
+ {
+ changed = updated_limits.max_velocity != parameter.get_value();
+ updated_limits.max_velocity = parameter.get_value();
+ }
+ else if (param_name == param_base_name + ".max_acceleration")
+ {
+ changed = updated_limits.max_acceleration != parameter.get_value();
+ updated_limits.max_acceleration = parameter.get_value();
+ }
+ else if (param_name == param_base_name + ".max_deceleration")
+ {
+ changed = updated_limits.max_deceleration != parameter.get_value();
+ updated_limits.max_deceleration = parameter.get_value();
+ }
+ else if (param_name == param_base_name + ".max_jerk")
+ {
+ changed = updated_limits.max_jerk != parameter.get_value();
+ updated_limits.max_jerk = parameter.get_value();
+ }
+ else if (param_name == param_base_name + ".max_effort")
+ {
+ changed = updated_limits.max_effort != parameter.get_value();
+ updated_limits.max_effort = parameter.get_value();
+ }
+ }
+ catch (const rclcpp::exceptions::InvalidParameterTypeException & e)
+ {
+ RCLCPP_WARN(logging_itf->get_logger(), "Please use the right type: %s", e.what());
+ }
+ }
+
+ for (auto & parameter : parameters)
+ {
+ const std::string param_name = parameter.get_name();
+ try
+ {
+ if (param_name == param_base_name + ".has_position_limits")
+ {
+ updated_limits.has_position_limits = parameter.get_value();
+ if (updated_limits.has_position_limits)
+ {
+ if (std::isnan(updated_limits.min_position) || std::isnan(updated_limits.max_position))
+ {
+ RCLCPP_WARN(
+ logging_itf->get_logger(),
+ "PARAMETER NOT UPDATED: Position limits can not be used, i.e., "
+ "'has_position_limits' flag can not be set, if 'min_position' "
+ "and 'max_position' are not set or not have valid double values.");
+ updated_limits.has_position_limits = false;
+ }
+ else if (updated_limits.min_position >= updated_limits.max_position)
+ {
+ RCLCPP_WARN(
+ logging_itf->get_logger(),
+ "PARAMETER NOT UPDATED: Position limits can not be used, i.e., "
+ "'has_position_limits' flag can not be set, if not "
+ "'min_position' < 'max_position'");
+ updated_limits.has_position_limits = false;
+ }
+ else
+ {
+ changed = true;
+ }
+ }
+ }
+ else if (param_name == param_base_name + ".has_velocity_limits")
+ {
+ updated_limits.has_velocity_limits = parameter.get_value();
+ if (updated_limits.has_velocity_limits && std::isnan(updated_limits.max_velocity))
+ {
+ RCLCPP_WARN(
+ logging_itf->get_logger(),
+ "PARAMETER NOT UPDATED: 'has_velocity_limits' flag can not be set if 'min_velocity' "
+ "and 'max_velocity' are not set or not have valid double values.");
+ updated_limits.has_velocity_limits = false;
+ }
+ else
+ {
+ changed = true;
+ }
+ }
+ else if (param_name == param_base_name + ".has_acceleration_limits")
+ {
+ updated_limits.has_acceleration_limits = parameter.get_value();
+ if (updated_limits.has_acceleration_limits && std::isnan(updated_limits.max_acceleration))
+ {
+ RCLCPP_WARN(
+ logging_itf->get_logger(),
+ "PARAMETER NOT UPDATED: 'has_acceleration_limits' flag can not be set if "
+ "'max_acceleration' is not set or not have valid double values.");
+ updated_limits.has_acceleration_limits = false;
+ }
+ else
+ {
+ changed = true;
+ }
+ }
+ else if (param_name == param_base_name + ".has_deceleration_limits")
+ {
+ updated_limits.has_deceleration_limits = parameter.get_value();
+ if (updated_limits.has_deceleration_limits && std::isnan(updated_limits.max_deceleration))
+ {
+ RCLCPP_WARN(
+ logging_itf->get_logger(),
+ "PARAMETER NOT UPDATED: 'has_deceleration_limits' flag can not be set if "
+ "'max_deceleration' is not set or not have valid double values.");
+ updated_limits.has_deceleration_limits = false;
+ }
+ else
+ {
+ changed = true;
+ }
+ }
+ else if (param_name == param_base_name + ".has_jerk_limits")
+ {
+ updated_limits.has_jerk_limits = parameter.get_value();
+ if (updated_limits.has_jerk_limits && std::isnan(updated_limits.max_jerk))
+ {
+ RCLCPP_WARN(
+ logging_itf->get_logger(),
+ "PARAMETER NOT UPDATED: 'has_jerk_limits' flag can not be set if 'max_jerk' is not set "
+ "or not have valid double values.");
+ updated_limits.has_jerk_limits = false;
+ }
+ else
+ {
+ changed = true;
+ }
+ }
+ else if (param_name == param_base_name + ".has_effort_limits")
+ {
+ updated_limits.has_effort_limits = parameter.get_value();
+ if (updated_limits.has_effort_limits && std::isnan(updated_limits.max_effort))
+ {
+ RCLCPP_WARN(
+ logging_itf->get_logger(),
+ "PARAMETER NOT UPDATED: 'has_effort_limits' flag can not be set if 'max_effort' is not "
+ "set or not have valid double values.");
+ updated_limits.has_effort_limits = false;
+ }
+ else
+ {
+ changed = true;
+ }
+ }
+ else if (param_name == param_base_name + ".angle_wraparound")
+ {
+ updated_limits.angle_wraparound = parameter.get_value();
+ if (updated_limits.angle_wraparound && updated_limits.has_position_limits)
+ {
+ RCLCPP_WARN(
+ logging_itf->get_logger(),
+ "PARAMETER NOT UPDATED: 'angle_wraparound' flag can not be set if "
+ "'has_position_limits' flag is set.");
+ updated_limits.angle_wraparound = false;
+ }
+ else
+ {
+ changed = true;
+ }
+ }
+ }
+ catch (const rclcpp::exceptions::InvalidParameterTypeException & e)
+ {
+ RCLCPP_WARN(
+ logging_itf->get_logger(), "PARAMETER NOT UPDATED: Please use the right type: %s",
+ e.what());
+ }
+ }
+
+ return changed;
+}
+
/// Populate a SoftJointLimits instance from the ROS parameter server.
/**
* It is assumed that the parameter structure is the following:
@@ -550,6 +755,85 @@ inline bool get_joint_limits(
lifecycle_node->get_node_logging_interface(), soft_limits);
}
+/**
+ * Check if any of updated parameters are related to SoftJointLimits.
+ *
+ * This method is intended to be used in the parameters update callback.
+ * It is recommended that it's result is temporarily stored and synchronized with the
+ * SoftJointLimits structure in the main loop.
+ *
+ * \param[in] joint_name Name of the joint whose limits should be checked.
+ * \param[in] parameters List of the parameters that should be checked if they belong to this
+ * structure and that are used for updating the internal values.
+ * \param[in] logging_itf node logging interface to provide log errors.
+ * \param[out] updated_limits structure with updated SoftJointLimits. It is recommended that the
+ * currently used limits are stored into this structure before calling this method.
+ */
+inline bool check_for_limits_update(
+ const std::string & joint_name, const std::vector & parameters,
+ const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
+ SoftJointLimits & updated_limits)
+{
+ const std::string param_base_name = "joint_limits." + joint_name;
+ bool changed = false;
+
+ for (auto & parameter : parameters)
+ {
+ const std::string param_name = parameter.get_name();
+ try
+ {
+ if (param_name == param_base_name + ".has_soft_limits")
+ {
+ if (!parameter.get_value())
+ {
+ RCLCPP_WARN(
+ logging_itf->get_logger(),
+ "Parameter 'has_soft_limits' is not set, therefore the limits will not be updated!");
+ return false;
+ }
+ }
+ }
+ catch (const rclcpp::exceptions::InvalidParameterTypeException & e)
+ {
+ RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what());
+ }
+ }
+
+ for (auto & parameter : parameters)
+ {
+ const std::string param_name = parameter.get_name();
+ try
+ {
+ if (param_name == param_base_name + ".k_position")
+ {
+ changed = updated_limits.k_position != parameter.get_value();
+ updated_limits.k_position = parameter.get_value();
+ }
+ else if (param_name == param_base_name + ".k_velocity")
+ {
+ changed = updated_limits.k_velocity != parameter.get_value();
+ updated_limits.k_velocity = parameter.get_value();
+ }
+ else if (param_name == param_base_name + ".soft_lower_limit")
+ {
+ changed = updated_limits.min_position != parameter.get_value();
+ updated_limits.min_position = parameter.get_value();
+ }
+ else if (param_name == param_base_name + ".soft_upper_limit")
+ {
+ changed = updated_limits.max_position != parameter.get_value();
+ updated_limits.max_position = parameter.get_value();
+ }
+ }
+ catch (const rclcpp::exceptions::InvalidParameterTypeException & e)
+ {
+ RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what());
+ }
+ }
+
+ return changed;
+}
+
} // namespace joint_limits
#endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_
diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp
new file mode 100644
index 0000000000..733178d695
--- /dev/null
+++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp
@@ -0,0 +1,105 @@
+// Copyright (c) 2023, PickNik Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \author Dr. Denis Stogl
+
+#ifndef JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_
+#define JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_
+
+#include
+#include
+#include
+#include
+
+#include "joint_limits/joint_limiter_interface.hpp"
+#include "joint_limits/joint_limits.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/duration.hpp"
+
+namespace joint_limits
+{
+/**
+ * Joint Saturation Limiter limits joints' position, velocity and acceleration by clamping values
+ * to its minimal and maximal allowed values. Since the position, velocity and accelerations are
+ * variables in physical relation, it might be that some values are limited lower then specified
+ * limit. For example, if a joint is close to its position limit, velocity and acceleration will be
+ * reduced accordingly.
+ */
+template
+class JointSaturationLimiter : public JointLimiterInterface
+{
+public:
+ /** \brief Constructor */
+ JOINT_LIMITS_PUBLIC JointSaturationLimiter();
+
+ /** \brief Destructor */
+ JOINT_LIMITS_PUBLIC ~JointSaturationLimiter();
+
+ JOINT_LIMITS_PUBLIC bool on_init() override { return true; }
+
+ JOINT_LIMITS_PUBLIC bool on_configure(
+ const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) override
+ {
+ return true;
+ }
+
+ /** \brief Enforce joint limits to desired position, velocity and acceleration using clamping.
+ * Class implements this method accepting following data types:
+ * - trajectory_msgs::msg::JointTrajectoryPoint: limiting position, velocity and acceleration;
+ *
+ * Implementation of saturation approach for joints with position, velocity or acceleration limits
+ * and values. First, position limits are checked to adjust desired velocity accordingly, then
+ * velocity and finally acceleration.
+ * The method support partial existence of limits, e.g., missing position limits for continuous
+ * joins.
+ *
+ * \param[in] current_joint_states current joint states a robot is in.
+ * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits.
+ * \param[in] dt time delta to calculate missing integrals and derivation in joint limits.
+ * \returns true if limits are enforced, otherwise false.
+ */
+ JOINT_LIMITS_PUBLIC bool on_enforce(
+ trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
+ trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states,
+ const rclcpp::Duration & dt) override;
+
+ /** \brief Enforce joint limits to desired arbitrary physical quantity.
+ * Additional, commonly used method for enforcing limits by clamping desired input value.
+ * The limit is defined in effort limits of the `joint::limits/JointLimit` structure.
+ *
+ * If `has_effort_limits` is set to false, the limits will be not enforced to a joint.
+ *
+ * \param[in,out] desired_joint_states physical quantity that should be adjusted to obey the
+ * limits. \returns true if limits are enforced, otherwise false.
+ */
+ JOINT_LIMITS_PUBLIC bool on_enforce(std::vector & desired_joint_states) override;
+
+private:
+ rclcpp::Clock::SharedPtr clock_;
+};
+
+template
+JointSaturationLimiter::JointSaturationLimiter() : JointLimiterInterface()
+{
+ clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME));
+}
+
+template
+JointSaturationLimiter::~JointSaturationLimiter()
+{
+}
+
+} // namespace joint_limits
+
+#endif // JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_
diff --git a/joint_limits/include/joint_limits/visibility_control.h b/joint_limits/include/joint_limits/visibility_control.h
new file mode 100644
index 0000000000..9c957a3cf9
--- /dev/null
+++ b/joint_limits/include/joint_limits/visibility_control.h
@@ -0,0 +1,49 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef JOINT_LIMITS__VISIBILITY_CONTROL_H_
+#define JOINT_LIMITS__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+// https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define JOINT_LIMITS_EXPORT __attribute__((dllexport))
+#define JOINT_LIMITS_IMPORT __attribute__((dllimport))
+#else
+#define JOINT_LIMITS_EXPORT __declspec(dllexport)
+#define JOINT_LIMITS_IMPORT __declspec(dllimport)
+#endif
+#ifdef JOINT_LIMITS_BUILDING_DLL
+#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_EXPORT
+#else
+#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_IMPORT
+#endif
+#define JOINT_LIMITS_PUBLIC_TYPE JOINT_LIMITS_PUBLIC
+#define JOINT_LIMITS_LOCAL
+#else
+#define JOINT_LIMITS_EXPORT __attribute__((visibility("default")))
+#define JOINT_LIMITS_IMPORT
+#if __GNUC__ >= 4
+#define JOINT_LIMITS_PUBLIC __attribute__((visibility("default")))
+#define JOINT_LIMITS_LOCAL __attribute__((visibility("hidden")))
+#else
+#define JOINT_LIMITS_PUBLIC
+#define JOINT_LIMITS_LOCAL
+#endif
+#define JOINT_LIMITS_PUBLIC_TYPE
+#endif
+
+#endif // JOINT_LIMITS__VISIBILITY_CONTROL_H_
diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml
new file mode 100644
index 0000000000..a49d88fbc9
--- /dev/null
+++ b/joint_limits/joint_limiters.xml
@@ -0,0 +1,11 @@
+
+
+
+
+ Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities.
+
+
+
+
diff --git a/joint_limits/package.xml b/joint_limits/package.xml
index a9a1d54dfd..921ec2b44f 100644
--- a/joint_limits/package.xml
+++ b/joint_limits/package.xml
@@ -1,10 +1,10 @@
joint_limits
- 4.8.0
- Interfaces for handling of joint limits for controllers or hardware.
+ 4.11.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
- Denis Štogl
+ Denis Štogl
Apache License 2.0
@@ -15,12 +15,19 @@
ament_cmake
ament_cmake_gen_version_h
+ backward_ros
+ pluginlib
+ realtime_tools
rclcpp
rclcpp_lifecycle
+ trajectory_msgs
urdf
- launch_testing_ament_cmake
+ ament_cmake_gmock
ament_cmake_gtest
+ generate_parameter_library
+ launch_ros
+ launch_testing_ament_cmake
ament_cmake
diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp
new file mode 100644
index 0000000000..843337259d
--- /dev/null
+++ b/joint_limits/src/joint_limiter_interface.cpp
@@ -0,0 +1,21 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \author Dr. Denis Stogl
+
+#include "joint_limits/joint_limiter_interface.hpp"
+
+namespace joint_limits
+{
+} // namespace joint_limits
diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp
new file mode 100644
index 0000000000..5020cab27b
--- /dev/null
+++ b/joint_limits/src/joint_saturation_limiter.cpp
@@ -0,0 +1,478 @@
+// Copyright (c) 2023, PickNik Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \authors Nathan Brooks, Dr. Denis Stogl, Guillaume Walck
+
+#include "joint_limits/joint_saturation_limiter.hpp"
+
+#include
+
+#include "rclcpp/duration.hpp"
+#include "rcutils/logging_macros.h"
+
+constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops
+constexpr double VALUE_CONSIDERED_ZERO = 1e-10;
+
+namespace joint_limits
+{
+template <>
+bool JointSaturationLimiter::on_enforce(
+ trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
+ trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt)
+{
+ bool limits_enforced = false;
+
+ const auto dt_seconds = dt.seconds();
+ // negative or null is not allowed
+ if (dt_seconds <= 0.0)
+ {
+ return false;
+ }
+
+ // TODO(gwalck) compute if the max are not implicitly violated with the given dt
+ // e.g. for max vel 2.0 and max acc 5.0, with dt >0.4
+ // velocity max is implicitly already violated due to max_acc * dt > 2.0
+
+ // check for required inputs combination
+ bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_);
+ bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_);
+ bool has_desired_acceleration = (desired_joint_states.accelerations.size() == number_of_joints_);
+ bool has_current_position = (current_joint_states.positions.size() == number_of_joints_);
+ bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_);
+
+ // pos state and vel or pos cmd is required, vel state is optional
+ if (!(has_current_position && (has_desired_position || has_desired_velocity)))
+ {
+ return false;
+ }
+
+ if (!has_current_velocity)
+ {
+ // First update() after activating does not have velocity available, assume 0
+ current_joint_states.velocities.resize(number_of_joints_, 0.0);
+ }
+
+ // TODO(destogl): please check if we get too much malloc from this initialization,
+ // if so then we should use members instead local variables and initialize them in other method
+ std::vector desired_pos(number_of_joints_);
+ std::vector desired_vel(number_of_joints_);
+ std::vector desired_acc(number_of_joints_);
+ std::vector expected_vel(number_of_joints_);
+ std::vector expected_pos(number_of_joints_);
+
+ // limits triggered
+ std::vector limited_jnts_pos, limited_jnts_vel, limited_jnts_acc, limited_jnts_dec;
+
+ bool braking_near_position_limit_triggered = false;
+
+ for (size_t index = 0; index < number_of_joints_; ++index)
+ {
+ if (has_desired_position)
+ {
+ desired_pos[index] = desired_joint_states.positions[index];
+ }
+ if (has_desired_velocity)
+ {
+ desired_vel[index] = desired_joint_states.velocities[index];
+ }
+ if (has_desired_acceleration)
+ {
+ desired_acc[index] = desired_joint_states.accelerations[index];
+ }
+
+ if (has_desired_position)
+ {
+ // limit position
+ if (joint_limits_[index].has_position_limits)
+ {
+ // clamp input pos_cmd
+ auto pos = std::clamp(
+ desired_pos[index], joint_limits_[index].min_position, joint_limits_[index].max_position);
+ if (pos != desired_pos[index])
+ {
+ desired_pos[index] = pos;
+ limited_jnts_pos.emplace_back(joint_names_[index]);
+ limits_enforced = true;
+ }
+ }
+ // priority to pos_cmd derivative over cmd_vel when not defined. If done always then we might
+ // get jumps in the velocity based on the system's dynamics. Position limit clamping is done
+ // below once again.
+ const double position_difference = desired_pos[index] - current_joint_states.positions[index];
+ if (
+ std::fabs(position_difference) > VALUE_CONSIDERED_ZERO &&
+ std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO)
+ {
+ desired_vel[index] = position_difference / dt_seconds;
+ }
+ }
+
+ // limit velocity
+ if (joint_limits_[index].has_velocity_limits)
+ {
+ // if desired velocity is not defined calculate it from positions
+ if (std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_vel[index]))
+ {
+ desired_vel[index] =
+ (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds;
+ }
+ // clamp input vel_cmd
+ if (std::fabs(desired_vel[index]) > joint_limits_[index].max_velocity)
+ {
+ desired_vel[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel[index]);
+ limited_jnts_vel.emplace_back(joint_names_[index]);
+ limits_enforced = true;
+
+ // recompute pos_cmd if needed
+ if (has_desired_position)
+ {
+ desired_pos[index] =
+ current_joint_states.positions[index] + desired_vel[index] * dt_seconds;
+ }
+
+ desired_acc[index] =
+ (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds;
+ }
+ }
+
+ // limit acceleration
+ if (
+ joint_limits_[index].has_acceleration_limits || joint_limits_[index].has_deceleration_limits)
+ {
+ // if(has_current_velocity)
+ if (1) // for now use a zero velocity if not provided
+ {
+ // limiting acc or dec function
+ auto apply_acc_or_dec_limit = [&](
+ const double max_acc_or_dec, std::vector & acc,
+ std::vector & limited_jnts) -> bool
+ {
+ if (std::fabs(acc[index]) > max_acc_or_dec)
+ {
+ acc[index] = std::copysign(max_acc_or_dec, acc[index]);
+ limited_jnts.emplace_back(joint_names_[index]);
+ limits_enforced = true;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ };
+
+ // if desired acceleration if not provided compute it from desired_vel and vel_state
+ if (
+ std::fabs(desired_acc[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc[index]))
+ {
+ desired_acc[index] =
+ (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds;
+ }
+
+ // check if decelerating - if velocity is changing toward 0
+ bool deceleration_limit_applied = false;
+ bool limit_applied = false;
+ if (
+ (desired_acc[index] < 0 && current_joint_states.velocities[index] > 0) ||
+ (desired_acc[index] > 0 && current_joint_states.velocities[index] < 0))
+ {
+ // limit deceleration
+ if (joint_limits_[index].has_deceleration_limits)
+ {
+ limit_applied = apply_acc_or_dec_limit(
+ joint_limits_[index].max_deceleration, desired_acc, limited_jnts_dec);
+ deceleration_limit_applied = true;
+ }
+ }
+
+ // limit acceleration (fallback to acceleration if no deceleration limits)
+ if (joint_limits_[index].has_acceleration_limits && !deceleration_limit_applied)
+ {
+ limit_applied = apply_acc_or_dec_limit(
+ joint_limits_[index].max_acceleration, desired_acc, limited_jnts_acc);
+ }
+
+ if (limit_applied)
+ {
+ // vel_cmd from integration of desired_acc, needed even if no vel output
+ desired_vel[index] =
+ current_joint_states.velocities[index] + desired_acc[index] * dt_seconds;
+ if (has_desired_position)
+ {
+ // pos_cmd from from double integration of desired_acc
+ desired_pos[index] = current_joint_states.positions[index] +
+ current_joint_states.velocities[index] * dt_seconds +
+ 0.5 * desired_acc[index] * dt_seconds * dt_seconds;
+ }
+ }
+ }
+ // else we cannot compute acc, so not limiting it
+ }
+
+ // plan ahead for position limits
+ if (joint_limits_[index].has_position_limits)
+ {
+ if (has_desired_velocity && !has_desired_position)
+ {
+ // Check immediate next step when using vel_cmd only, other cases already handled
+ // integrate pos
+ expected_pos[index] =
+ current_joint_states.positions[index] + desired_vel[index] * dt_seconds;
+ // if expected_pos over limit
+ auto pos = std::clamp(
+ expected_pos[index], joint_limits_[index].min_position,
+ joint_limits_[index].max_position);
+ if (pos != expected_pos[index])
+ {
+ // TODO(gwalck) compute vel_cmd that would permit to slow down in time at full
+ // deceleration in any case limit pos to max
+ expected_pos[index] = pos;
+ // and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be
+ // zero)
+ desired_vel[index] =
+ (expected_pos[index] - current_joint_states.positions[index]) / dt_seconds;
+ limited_jnts_pos.emplace_back(joint_names_[index]);
+ limits_enforced = true;
+ }
+ }
+
+ // Check that stopping distance is within joint limits
+ // Slow down all joints at maximum decel if any don't have stopping distance within joint
+ // limits
+
+ // delta_x = (v2*v2 - v1*v1) / (2*a)
+ // stopping_distance = (- v1*v1) / (2*max_acceleration)
+ // Here we assume we will not trigger velocity limits while maximally decelerating.
+ // This is a valid assumption if we are not currently at a velocity limit since we are just
+ // coming to a rest.
+ double stopping_deccel = std::fabs(desired_vel[index] / dt_seconds);
+ if (joint_limits_[index].has_deceleration_limits)
+ {
+ stopping_deccel = joint_limits_[index].max_deceleration;
+ }
+ else if (joint_limits_[index].has_acceleration_limits)
+ {
+ stopping_deccel = joint_limits_[index].max_acceleration;
+ }
+
+ double stopping_distance =
+ std::fabs((-desired_vel[index] * desired_vel[index]) / (2 * stopping_deccel));
+ // compute stopping duration at stopping_deccel
+ double stopping_duration = std::fabs((desired_vel[index]) / (stopping_deccel));
+
+ // Check that joint limits are beyond stopping_distance and desired_velocity is towards
+ // that limit
+ if (
+ (desired_vel[index] < 0 &&
+ (current_joint_states.positions[index] - joint_limits_[index].min_position <
+ stopping_distance)) ||
+ (desired_vel[index] > 0 &&
+ (joint_limits_[index].max_position - current_joint_states.positions[index] <
+ stopping_distance)))
+ {
+ limited_jnts_pos.emplace_back(joint_names_[index]);
+ braking_near_position_limit_triggered = true;
+ limits_enforced = true;
+ }
+ else
+ {
+ // compute the travel_distance at new desired velocity, with best case duration
+ // stopping_duration
+ double motion_after_stopping_duration = desired_vel[index] * stopping_duration;
+ // re-check what happens if we don't slow down
+ if (
+ (desired_vel[index] < 0 &&
+ (current_joint_states.positions[index] - joint_limits_[index].min_position <
+ motion_after_stopping_duration)) ||
+ (desired_vel[index] > 0 &&
+ (joint_limits_[index].max_position - current_joint_states.positions[index] <
+ motion_after_stopping_duration)))
+ {
+ limited_jnts_pos.emplace_back(joint_names_[index]);
+ braking_near_position_limit_triggered = true;
+ limits_enforced = true;
+ }
+ // else no need to slow down. in worse case we won't hit the limit at current velocity
+ }
+ }
+ }
+
+ // update variables according to triggers
+ if (braking_near_position_limit_triggered)
+ {
+ // this limit applies to all joints even if a single one is triggered
+ for (size_t index = 0; index < number_of_joints_; ++index)
+ {
+ // Compute accel to stop
+ // Here we aren't explicitly maximally decelerating, but for joints near their limits this
+ // should still result in max decel being used
+ desired_acc[index] = -current_joint_states.velocities[index] / dt_seconds;
+ if (joint_limits_[index].has_deceleration_limits)
+ {
+ desired_acc[index] = std::copysign(
+ std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_deceleration),
+ desired_acc[index]);
+ }
+ else if (joint_limits_[index].has_acceleration_limits)
+ {
+ desired_acc[index] = std::copysign(
+ std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_acceleration),
+ desired_acc[index]);
+ }
+
+ // Recompute velocity and position
+ if (has_desired_velocity)
+ {
+ desired_vel[index] =
+ current_joint_states.velocities[index] + desired_acc[index] * dt_seconds;
+ }
+ if (has_desired_position)
+ {
+ desired_pos[index] = current_joint_states.positions[index] +
+ current_joint_states.velocities[index] * dt_seconds +
+ 0.5 * desired_acc[index] * dt_seconds * dt_seconds;
+ }
+ }
+ std::ostringstream ostr;
+ for (auto jnt : limited_jnts_pos)
+ {
+ ostr << jnt << " ";
+ }
+ ostr << "\b \b"; // erase last character
+ RCLCPP_WARN_STREAM_THROTTLE(
+ node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+ "Joint(s) [" << ostr.str().c_str()
+ << "] would exceed position limits"
+ " if continuing at current state, limiting all joints");
+ }
+
+ // display limitations
+
+ // if position limiting
+ if (limited_jnts_pos.size() > 0)
+ {
+ std::ostringstream ostr;
+ for (auto jnt : limited_jnts_pos)
+ {
+ ostr << jnt << " ";
+ }
+ ostr << "\b \b"; // erase last character
+ RCLCPP_WARN_STREAM_THROTTLE(
+ node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+ "Joint(s) [" << ostr.str().c_str() << "] would exceed position limits, limiting");
+ }
+
+ if (limited_jnts_vel.size() > 0)
+ {
+ std::ostringstream ostr;
+ for (auto jnt : limited_jnts_vel)
+ {
+ ostr << jnt << " ";
+ }
+ ostr << "\b \b"; // erase last character
+ RCLCPP_WARN_STREAM_THROTTLE(
+ node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+ "Joint(s) [" << ostr.str().c_str() << "] would exceed velocity limits, limiting");
+ }
+
+ if (limited_jnts_acc.size() > 0)
+ {
+ std::ostringstream ostr;
+ for (auto jnt : limited_jnts_acc)
+ {
+ ostr << jnt << " ";
+ }
+ ostr << "\b \b"; // erase last character
+ RCLCPP_WARN_STREAM_THROTTLE(
+ node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+ "Joint(s) [" << ostr.str().c_str() << "] would exceed acceleration limits, limiting");
+ }
+
+ if (limited_jnts_dec.size() > 0)
+ {
+ std::ostringstream ostr;
+ for (auto jnt : limited_jnts_dec)
+ {
+ ostr << jnt << " ";
+ }
+ ostr << "\b \b"; // erase last character
+ RCLCPP_WARN_STREAM_THROTTLE(
+ node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+ "Joint(s) [" << ostr.str().c_str() << "] would exceed deceleration limits, limiting");
+ }
+
+ if (has_desired_position)
+ {
+ desired_joint_states.positions = desired_pos;
+ }
+ if (has_desired_velocity)
+ {
+ desired_joint_states.velocities = desired_vel;
+ }
+ if (has_desired_acceleration)
+ {
+ desired_joint_states.accelerations = desired_acc;
+ }
+
+ return limits_enforced;
+}
+
+template <>
+bool JointSaturationLimiter::on_enforce(std::vector & desired_joint_states)
+{
+ std::vector limited_joints_effort;
+ bool limits_enforced = false;
+
+ bool has_cmd = (desired_joint_states.size() == number_of_joints_);
+ if (!has_cmd)
+ {
+ return false;
+ }
+
+ for (size_t index = 0; index < number_of_joints_; ++index)
+ {
+ if (joint_limits_[index].has_effort_limits)
+ {
+ double max_effort = joint_limits_[index].max_effort;
+ if (std::fabs(desired_joint_states[index]) > max_effort)
+ {
+ desired_joint_states[index] = std::copysign(max_effort, desired_joint_states[index]);
+ limited_joints_effort.emplace_back(joint_names_[index]);
+ limits_enforced = true;
+ }
+ }
+ }
+
+ if (limited_joints_effort.size() > 0)
+ {
+ std::ostringstream ostr;
+ for (auto jnt : limited_joints_effort)
+ {
+ ostr << jnt << " ";
+ }
+ ostr << "\b \b"; // erase last character
+ RCLCPP_WARN_STREAM_THROTTLE(
+ node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD,
+ "Joint(s) [" << ostr.str().c_str() << "] would exceed effort limits, limiting");
+ }
+
+ return limits_enforced;
+}
+
+} // namespace joint_limits
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+ joint_limits::JointSaturationLimiter,
+ joint_limits::JointLimiterInterface)
diff --git a/joint_limits/test/joint_saturation_limiter_param.yaml b/joint_limits/test/joint_saturation_limiter_param.yaml
new file mode 100644
index 0000000000..f025421519
--- /dev/null
+++ b/joint_limits/test/joint_saturation_limiter_param.yaml
@@ -0,0 +1,57 @@
+joint_saturation_limiter:
+ ros__parameters:
+ joint_limits:
+ # Get full specification from parameter server
+ foo_joint:
+ has_position_limits: true
+ min_position: -5.0
+ max_position: 5.0
+ has_velocity_limits: true
+ max_velocity: 2.0
+ has_acceleration_limits: true
+ max_acceleration: 5.0
+ has_deceleration_limits: true
+ max_deceleration: 7.5
+ has_jerk_limits: true
+ max_jerk: 100.0
+ has_effort_limits: true
+ max_effort: 20.0
+ angle_wraparound: true # should be ignored, has position limits
+ has_soft_limits: true
+ k_position: 10.0
+ k_velocity: 20.0
+ soft_lower_limit: 0.1
+ soft_upper_limit: 0.9
+
+ foo_joint_no_effort:
+ has_position_limits: true
+ min_position: -5.0
+ max_position: 5.0
+ has_velocity_limits: true
+ max_velocity: 2.0
+ has_acceleration_limits: true
+ max_acceleration: 5.0
+ has_deceleration_limits: true
+ max_deceleration: 7.5
+ has_jerk_limits: true
+ max_jerk: 100.0
+ has_effort_limits: false
+ max_effort: 20.0
+ angle_wraparound: true # should be ignored, has position limits
+ has_soft_limits: true
+ k_position: 10.0
+ k_velocity: 20.0
+ soft_lower_limit: 0.1
+ soft_upper_limit: 0.9
+
+joint_saturation_limiter_nodeclimit:
+ ros__parameters:
+ joint_limits:
+ foo_joint:
+ has_position_limits: true
+ min_position: -5.0
+ max_position: 5.0
+ has_velocity_limits: true
+ max_velocity: 2.0
+ has_acceleration_limits: true
+ max_acceleration: 5.0
diff --git a/joint_limits/test/test_joint_saturation_limiter.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp
new file mode 100644
index 0000000000..e78c4b8994
--- /dev/null
+++ b/joint_limits/test/test_joint_saturation_limiter.cpp
@@ -0,0 +1,569 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \author Dr. Denis Stogl, Guillaume Walck
+
+#include "test_joint_saturation_limiter.hpp"
+
+TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded)
+{
+ // Test JointSaturationLimiter loading
+ ASSERT_NO_THROW(
+ joint_limiter_ = std::unique_ptr(
+ joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)));
+ ASSERT_NE(joint_limiter_, nullptr);
+}
+
+// NOTE: We accept also if there is no limit defined for a joint.
+TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ // initialize the limiter
+ std::vector joint_names = {"bar_joint"};
+ ASSERT_TRUE(joint_limiter_->init(joint_names, node_));
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+ rclcpp::Duration period(0, 0); // 0 second
+ ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ // no size check occurs (yet) so expect true
+ ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
+
+ rclcpp::Duration period(1, 0); // 1 second
+ // test no desired interface
+ desired_joint_states_.positions.clear();
+ desired_joint_states_.velocities.clear();
+ // currently not handled desired_joint_states_.accelerations.clear();
+ ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ joint_limiter_->configure(last_commanded_state_);
+
+ rclcpp::Duration period(1, 0); // 1 second
+ // test no position interface
+ current_joint_states_.positions.clear();
+ ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // also fail if out of limits
+ desired_joint_states_.positions[0] = 20.0;
+ ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_no_velstate_expect_limiting)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ joint_limiter_->configure(last_commanded_state_);
+
+ rclcpp::Duration period(1, 0); // 1 second
+ // test no vel interface
+ current_joint_states_.velocities.clear();
+ ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+ // also fail if out of limits
+ desired_joint_states_.positions[0] = 20.0;
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ rclcpp::Duration period(1.0, 0.0); // 1 second
+ // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5
+
+ // within limits
+ desired_joint_states_.positions[0] = 1.0;
+ desired_joint_states_.velocities[0] = 1.0; // valid pos derivatite as well
+ ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if no limits applied
+ CHECK_STATE_SINGLE_JOINT(
+ desired_joint_states_, 0,
+ 1.0, // pos unchanged
+ 1.0, // vel unchanged
+ 1.0 // acc = vel / 1.0
+ );
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied_with_acc)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ rclcpp::Duration period(1.0, 0.0); // 1 second
+ // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5
+
+ // within limits
+ desired_joint_states_.positions[0] = 1.0;
+ desired_joint_states_.velocities[0] = 1.5; // valid pos derivative as well
+ desired_joint_states_.accelerations[0] = 2.9; // valid pos derivative as well
+ ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if no limits applied
+ CHECK_STATE_SINGLE_JOINT(
+ desired_joint_states_, 0,
+ 1.0, // pos unchanged
+ 1.5, // vel unchanged
+ 2.9 // acc = vel / 1.0
+ );
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ rclcpp::Duration period(1.0, 0.0); // 1 second
+
+ // pos/vel cmd ifs
+ current_joint_states_.positions[0] = -2.0;
+ desired_joint_states_.positions[0] = 1.0;
+ // desired velocity exceeds although correctly computed from pos derivative
+ desired_joint_states_.velocities[0] = 3.0;
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if limits applied
+ CHECK_STATE_SINGLE_JOINT(
+ desired_joint_states_, 0,
+ 0.0, // pos = pos + max_vel * dt
+ 2.0, // vel limited to max_vel
+ 2.0 / 1.0 // acc set to vel change/DT
+ );
+
+ // check opposite velocity direction (sign copy)
+ current_joint_states_.positions[0] = 2.0;
+ desired_joint_states_.positions[0] = -1.0;
+ // desired velocity exceeds although correctly computed from pos derivative
+ desired_joint_states_.velocities[0] = -3.0;
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if vel and acc limits applied
+ CHECK_STATE_SINGLE_JOINT(
+ desired_joint_states_, 0,
+ 0.0, // pos = pos - max_vel * dt
+ -2.0, // vel limited to -max_vel
+ -2.0 / 1.0 // acc set to vel change/DT
+ );
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc_enforced)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ rclcpp::Duration period(1.0, 0.0); // 1 second
+
+ // set current velocity close to limits to not be blocked by max acc to reach max
+ current_joint_states_.velocities[0] = 1.9;
+ // desired pos leads to vel exceeded (4.0 / sec instead of 2.0)
+ desired_joint_states_.positions[0] = 4.0;
+ // no vel cmd (will lead to internal computation of velocity)
+ desired_joint_states_.velocities.clear();
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if pos and acc limits applied
+ ASSERT_EQ(desired_joint_states_.positions[0], 2.0); // pos limited to max_vel * DT
+ // no vel cmd ifs
+ ASSERT_EQ(
+ desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT
+
+ // check opposite position and direction
+ current_joint_states_.positions[0] = 0.0;
+ current_joint_states_.velocities[0] = -1.9;
+ desired_joint_states_.positions[0] = -4.0;
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if pos and acc limits applied
+ ASSERT_EQ(desired_joint_states_.positions[0], -2.0); // pos limited to -max_vel * DT
+ // no vel cmd ifs
+ ASSERT_EQ(
+ desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc_enforced)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ rclcpp::Duration period(1.0, 0.0); // 1 second
+
+ // vel cmd ifs
+ current_joint_states_.positions[0] = -2.0;
+ // set current velocity close to limits to not be blocked by max acc to reach max
+ current_joint_states_.velocities[0] = 1.9;
+ // no pos cmd
+ desired_joint_states_.positions.clear();
+ // desired velocity exceeds
+ desired_joint_states_.velocities[0] = 3.0;
+
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if vel and acc limits applied
+ ASSERT_EQ(desired_joint_states_.velocities[0], 2.0); // vel limited to max_vel
+ // no vel cmd ifs
+ ASSERT_EQ(
+ desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT
+
+ // check opposite velocity direction
+ current_joint_states_.positions[0] = 2.0;
+ // set current velocity close to limits to not be blocked by max acc to reach max
+ current_joint_states_.velocities[0] = -1.9;
+ // desired velocity exceeds
+ desired_joint_states_.velocities[0] = -3.0;
+
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if vel and acc limits applied
+ ASSERT_EQ(desired_joint_states_.velocities[0], -2.0); // vel limited to -max_vel
+ // no vel cmd ifs
+ ASSERT_EQ(
+ desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_posonly_exceeded_expect_pos_enforced)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ rclcpp::Duration period(1.0, 0.0); // 1 second
+
+ // desired pos exceeds
+ current_joint_states_.positions[0] = 5.0;
+ desired_joint_states_.positions[0] = 20.0;
+ // no velocity interface
+ desired_joint_states_.velocities.clear();
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if pos limits applied
+ ASSERT_EQ(
+ desired_joint_states_.positions[0], 5.0); // pos limited clamped (was already at limit)
+ // no vel cmd ifs
+ ASSERT_EQ(desired_joint_states_.accelerations[0], 0.0); // acc set to vel change/DT
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ // using 0.05 because 1.0 sec invalidates the "small dt integration"
+ rclcpp::Duration period(0, 50000000); // 0.05 second
+
+ // close to pos limit should reduce velocity and stop
+ current_joint_states_.positions[0] = 4.851;
+ current_joint_states_.velocities[0] = 1.5;
+ desired_joint_states_.positions[0] = 4.926;
+ desired_joint_states_.velocities[0] = 1.5;
+
+ // this setup requires 0.15 distance to stop, and 0.2 seconds (so 4 cycles at 0.05)
+ std::vector expected_ret = {true, true, true, false};
+ for (auto i = 0u; i < 4; ++i)
+ {
+ auto previous_vel_request = desired_joint_states_.velocities[0];
+ // expect limits applied until the end stop
+ ASSERT_EQ(
+ joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period),
+ expected_ret[i]);
+
+ ASSERT_LE(
+ desired_joint_states_.velocities[0],
+ previous_vel_request); // vel adapted to reach end-stop should be decreasing
+ // NOTE: after the first cycle, vel is reduced and does not trigger stopping position limit
+ // hence no max deceleration anymore...
+ ASSERT_LT(
+ desired_joint_states_.positions[0],
+ 5.0 + COMMON_THRESHOLD); // should decelerate at each cycle and stay below limits
+ ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate
+
+ Integrate(period.seconds());
+
+ ASSERT_LT(current_joint_states_.positions[0], 5.0); // below joint limit
+ }
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_enforced)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ rclcpp::Duration period(0, 50000000);
+
+ // desired acceleration exceeds
+ current_joint_states_.positions[0] = 0.1;
+ current_joint_states_.velocities[0] = 0.1;
+ desired_joint_states_.positions[0] = 0.125; // valid pos cmd for the desired velocity
+ desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if limits applied
+ CHECK_STATE_SINGLE_JOINT(
+ desired_joint_states_, 0,
+ 0.11125, // pos = double integration from max acc with current state
+ 0.35, // vel limited to vel + max acc * dt
+ 5.0 // acc max acc
+ );
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_enforced)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ rclcpp::Duration period(0, 50000000);
+
+ // desired acceleration exceeds
+ current_joint_states_.positions[0] = 0.1;
+ current_joint_states_.velocities[0] = 0.1;
+ desired_joint_states_.positions[0] =
+ 0.125; // pos cmd leads to vel 0.5 that leads to acc > max acc
+ desired_joint_states_.velocities.clear();
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if pos and acc limits applied
+ ASSERT_NEAR(
+ desired_joint_states_.positions[0], 0.111250,
+ COMMON_THRESHOLD); // pos = double integration from max acc with current state
+ // no vel cmd ifs
+ ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_enforced)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ rclcpp::Duration period(0, 50000000);
+
+ // desired acceleration exceeds
+ current_joint_states_.positions[0] = 0.1;
+ current_joint_states_.velocities[0] = 0.1;
+ desired_joint_states_.positions.clear(); // = 0.125;
+ desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if pos and acc limits applied
+ // no pos cmd ifs
+ ASSERT_EQ(desired_joint_states_.velocities[0], 0.35); // vel limited to vel + max acc * dt
+ ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_expect_dec_enforced)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ rclcpp::Duration period(0, 50000000);
+
+ // desired deceleration exceeds
+ current_joint_states_.positions[0] = 0.3;
+ current_joint_states_.velocities[0] = 0.5;
+ desired_joint_states_.positions[0] = 0.305; // valid pos cmd for the desired velocity
+ desired_joint_states_.velocities[0] = 0.1; // leads to acc < -max dec
+
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if vel and acc limits applied
+ CHECK_STATE_SINGLE_JOINT(
+ desired_joint_states_, 0,
+ 0.315625, // pos = double integration from max dec with current state
+ 0.125, // vel limited by vel - max dec * dt
+ -7.5 // acc limited by -max dec
+ );
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced)
+{
+ SetupNode("joint_saturation_limiter_nodeclimit");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ rclcpp::Duration period(0, 50000000);
+
+ // desired deceleration exceeds
+ current_joint_states_.positions[0] = 1.0;
+ current_joint_states_.velocities[0] = 1.0;
+ desired_joint_states_.positions[0] = 1.025; // valid pos cmd for the desired decreased velocity
+ desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc
+ ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period));
+
+ // check if vel and acc limits applied
+ CHECK_STATE_SINGLE_JOINT(
+ desired_joint_states_, 0,
+ 1.04375, // pos = double integration from max acc with current state
+ 0.75, // vel limited by vel-max acc * dt
+ -5.0 // acc limited to -max acc
+ );
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_there_are_effort_limits_expect_them_to_be_applyed)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init();
+ Configure();
+
+ // value not over limit
+ desired_joint_states_.effort[0] = 15.0;
+ ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort));
+
+ // value over limit
+ desired_joint_states_.effort[0] = 21.0;
+ ASSERT_TRUE(joint_limiter_->enforce(desired_joint_states_.effort));
+ ASSERT_EQ(desired_joint_states_.effort[0], 20.0);
+ }
+}
+
+TEST_F(JointSaturationLimiterTest, when_there_are_no_effort_limits_expect_them_not_applyed)
+{
+ SetupNode("joint_saturation_limiter");
+ Load();
+
+ if (joint_limiter_)
+ {
+ Init("foo_joint_no_effort");
+ Configure();
+
+ // value not over limit
+ desired_joint_states_.effort[0] = 15.0;
+ ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort));
+
+ // value over limit
+ desired_joint_states_.effort[0] = 21.0;
+ ASSERT_FALSE(joint_limiter_->enforce(desired_joint_states_.effort));
+ ASSERT_EQ(desired_joint_states_.effort[0], 21.0);
+ }
+}
+
+int main(int argc, char ** argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ rclcpp::init(argc, argv);
+ int result = RUN_ALL_TESTS();
+ rclcpp::shutdown();
+ return result;
+}
diff --git a/joint_limits/test/test_joint_saturation_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp
new file mode 100644
index 0000000000..950b611109
--- /dev/null
+++ b/joint_limits/test/test_joint_saturation_limiter.hpp
@@ -0,0 +1,107 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TEST_JOINT_SATURATION_LIMITER_HPP_
+#define TEST_JOINT_SATURATION_LIMITER_HPP_
+
+#include
+
+#include
+#include