diff --git a/.clang-format b/.clang-format index 9c21e1e17c..698bda7da8 100644 --- a/.clang-format +++ b/.clang-format @@ -10,6 +10,6 @@ ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 DerivePointerAlignment: false PointerAlignment: Middle -ReflowComments: false +ReflowComments: true IncludeBlocks: Preserve ... diff --git a/.github/PULL_REQUEST_TEMPLATE/pull_request_template.md b/.github/pull_request_template.md similarity index 92% rename from .github/PULL_REQUEST_TEMPLATE/pull_request_template.md rename to .github/pull_request_template.md index 87f389a124..8e9178c167 100644 --- a/.github/PULL_REQUEST_TEMPLATE/pull_request_template.md +++ b/.github/pull_request_template.md @@ -1,13 +1,3 @@ - ---- -name: Pull request -about: Create a pull request -title: '' -labels: '' -assignees: '' - ---- - Contributions via pull requests are much appreciated. Before sending us a pull request, please ensure that: 1. Limited scope. Your PR should do one thing or one set of things. Avoid adding “random fixes” to PRs. Put those on separate PRs. diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index 50c707e12b..84b156f5a1 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -16,13 +16,16 @@ groups: - rosterloh - progtologist - arne48 + - christophfroehlich - DasRoteSkelett - - Serafadam + - sgmurray - harderthan - jaron-l - malapatiravi - - homalozoa - erickisos + - sachinkum0009 + - qiayuanliao + - homalozoa - anfemosa - jackcenter - VX792 @@ -31,6 +34,7 @@ groups: - aprotyas - peterdavidfagan - duringhof + - VanshGehlot - bijoua29 - - lm2292 + - LukasMacha97 - mcbed diff --git a/.github/workflows/README.md b/.github/workflows/README.md index fd751faeef..e784883593 100644 --- a/.github/workflows/README.md +++ b/.github/workflows/README.md @@ -1,7 +1,7 @@ ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_control/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-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) -**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-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/#humble) +**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-testing.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-testing.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-source-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) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-testing.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-testing.yml?branch=master)
[![Humble Source Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-source-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) **Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-testing.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-testing.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-testing.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/index.html)
[API Reference](https://control.ros.org/galactic/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#galactic) **Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-testing.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-testing.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-testing.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/index.html)
[API Reference](https://control.ros.org/foxy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#foxy) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 62ccd56b0c..1df5175f10 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,7 +16,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 @@ -44,7 +44,7 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.2 + - uses: codecov/codecov-action@v3.1.4 with: file: ros_ws/lcov/total_coverage.info flags: unittests diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 3e6def8be3..4cf7330f56 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -13,11 +13,11 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.5.0 + - uses: actions/setup-python@v4.6.1 with: python-version: '3.10' - name: Install system hooks - run: sudo apt install -qq clang-format-14 cppcheck + run: sudo apt install -qq cppcheck - uses: pre-commit/action@v3.0.0 with: extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 67d1b2c5e9..57fe01c1a4 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -36,7 +36,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 5a715fd503..777d193c69 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.6.1 + - uses: ros-tooling/setup-ros@0.6.2 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 4af4875830..0ad4d0aa6c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -36,7 +36,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.3.1 + rev: v3.4.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -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: 23.1.0 + rev: 23.3.0 hooks: - id: black args: ["--line-length=99"] @@ -61,15 +61,10 @@ repos: args: ["--extend-ignore=E501"] # CPP hooks - - repo: local + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.6 hooks: - id: clang-format - name: clang-format - description: Format files with ClangFormat. - entry: clang-format-14 - language: system - files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: ['-fallback-style=none', '-i'] - repo: local hooks: @@ -133,7 +128,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.2 + rev: v2.2.4 hooks: - id: codespell args: ['--write-changes'] diff --git a/README.md b/README.md index f19f01cc10..276ffb9e6a 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,8 @@ For more, please check the [documentation](https://ros-controls.github.io/contro ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`rolling`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.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) -**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/master) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.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/#humble) +**Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.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) +**Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.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) **Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-binary-build-main.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control/actions/workflows/galactic-semi-binary-build-main.yml?branch=galactic) | [Documentation](https://control.ros.org/galactic/index.html)
[API Reference](https://control.ros.org/galactic/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#galactic) **Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-binary-build-main.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control/actions/workflows/foxy-semi-binary-build-main.yml?branch=foxy) | [Documentation](https://control.ros.org/foxy/index.html)
[API Reference](https://control.ros.org/foxy/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#foxy) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index a81a293ff3..29f69150d2 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + 3.12.1 (2023-04-14) ------------------- * Add missing build_export_depends to controller_interface (`#989 `_) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 37b784011c..2bdccefdc5 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -41,7 +41,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual ~ChainableControllerInterface() = default; /** - * Control step update. Command interfaces are updated based on on reference inputs and current states. + * Control step update. Command interfaces are updated based on on reference inputs and current + * states. * **The method called in the (real-time) control loop.** * * \param[in] time The time at the start of this control loop iteration @@ -83,7 +84,9 @@ class ChainableControllerInterface : public ControllerInterfaceBase * * \param[in] flag marking a switch to or from chained mode. * - * \returns true if controller successfully switched between "chained" and "external" mode. \default returns true so the method don't have to be overridden if controller can always switch chained mode. + * \returns true if controller successfully switched between "chained" and "external" mode. + * \default returns true so the method don't have to be overridden if controller can always switch + * chained mode. */ virtual bool on_set_chained_mode(bool chained_mode); diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index fdaffac06f..05ee0c8830 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -74,15 +74,13 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy /// Get configuration for controller's required command interfaces. /** - * Method used by the controller_manager to get the set of command interfaces used by the controller. - * Each controller can use individual method to determine interface names that in simples case - * have the following format: `/`. - * The method is called only in `inactive` or `active` state, i.e., `on_configure` has to be - * called first. - * The configuration is used to check if controller can be activated and to claim interfaces from - * hardware. - * The claimed interfaces are populated in the - * \ref ControllerInterfaceBase::command_interfaces_ "command_interfaces_" member. + * Method used by the controller_manager to get the set of command interfaces used by the + * controller. Each controller can use individual method to determine interface names that in + * simples case have the following format: `/`. The method is called only in + * `inactive` or `active` state, i.e., `on_configure` has to be called first. The configuration is + * used to check if controller can be activated and to claim interfaces from hardware. The claimed + * interfaces are populated in the \ref ControllerInterfaceBase::command_interfaces_ + * "command_interfaces_" member. * * \returns configuration of command interfaces. */ @@ -131,7 +129,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy virtual CallbackReturn on_init() = 0; /** - * Control step update. Command interfaces are updated based on on reference inputs and current states. + * Control step update. Command interfaces are updated based on on reference inputs and current + * states. * **The method called in the (real-time) control loop.** * * \param[in] time The time at the start of this control loop iteration diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index 45b048fc72..b571751f55 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -23,19 +23,19 @@ namespace controller_interface { /// Reorder interfaces with references according to joint names or full interface names. /** - * Method to reorder and check if all expected interfaces are provided for the joint. - * Fill `ordered_interfaces` with references from `unordered_interfaces` in the same order as in - * `ordered_names`. - * - * \param[in] unordered_interfaces vector with loaned unordered state or command interfaces. - * \param[in] ordered_names vector with ordered names to order \p unordered_interfaces. - * The valued inputs are list of joint names or interface full names. - * If joint names are used for ordering, \p interface_type specifies valid interface. - * If full interface names are used for ordering, \p interface_type should be empty string (""). - * \param[in] interface_type used for ordering interfaces with respect to joint names. - * \param[out] ordered_interfaces vector with ordered interfaces. - * \return true if all interfaces or joints in \p ordered_names are found, otherwise false. - */ + * Method to reorder and check if all expected interfaces are provided for the joint. + * Fill `ordered_interfaces` with references from `unordered_interfaces` in the same order as in + * `ordered_names`. + * + * \param[in] unordered_interfaces vector with loaned unordered state or command interfaces. + * \param[in] ordered_names vector with ordered names to order \p unordered_interfaces. + * The valued inputs are list of joint names or interface full names. + * If joint names are used for ordering, \p interface_type specifies valid interface. + * If full interface names are used for ordering, \p interface_type should be empty string (""). + * \param[in] interface_type used for ordering interfaces with respect to joint names. + * \param[out] ordered_interfaces vector with ordered interfaces. + * \return true if all interfaces or joints in \p ordered_names are found, otherwise false. + */ template bool get_ordered_interfaces( std::vector & unordered_interfaces, const std::vector & ordered_names, diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 510c53d057..f20eccd9eb 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.12.1 + 3.13.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 8372b0db3f..d663cd3b32 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.13.0 (2023-05-18) +------------------- +* Add class for thread management of async hw interfaces (`#981 `_) +* Fix GitHub link on control.ros.org (`#1022 `_) +* Remove log-level argument from spawner script (`#1013 `_) +* Contributors: Christoph Fröhlich, Márk Szitanics, Bijou Abraham + +3.12.2 (2023-04-29) +------------------- + 3.12.1 (2023-04-14) ------------------- diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 24aad0ae78..8463c11b19 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -184,13 +184,6 @@ def main(args=None): default=10, type=int, ) - parser.add_argument( - "--log-level", - help="Log level for spawner node", - required=False, - choices=["debug", "info", "warn", "error", "fatal"], - default="info", - ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) @@ -200,15 +193,6 @@ def main(args=None): param_file = args.param_file controller_type = args.controller_type controller_manager_timeout = args.controller_manager_timeout - log_level = args.log_level - - loglevel_to_severity = { - "debug": rclpy.logging.LoggingSeverity.DEBUG, - "info": rclpy.logging.LoggingSeverity.INFO, - "warn": rclpy.logging.LoggingSeverity.WARN, - "error": rclpy.logging.LoggingSeverity.ERROR, - "fatal": rclpy.logging.LoggingSeverity.FATAL, - } if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) @@ -218,7 +202,6 @@ def main(args=None): prefixed_controller_name = controller_namespace + "/" + controller_name node = Node("spawner_" + controller_name) - rclpy.logging.set_logger_level("spawner_" + controller_name, loglevel_to_severity[log_level]) if not controller_manager_name.startswith("/"): spawner_namespace = node.get_namespace() diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index b23ed876b4..de42850b6b 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_manager/doc/userdoc.rst + .. _controller_manager_userdoc: Controller Manager diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 2c0d1d8fab..f05de22d9c 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -326,8 +326,8 @@ class ControllerManager : public rclcpp::Node const std::string & command_interface); /** - * Clear request lists used when switching controllers. The lists are shared between "callback" and - * "control loop" threads. + * Clear request lists used when switching controllers. The lists are shared between "callback" + * and "control loop" threads. */ void clear_requests(); @@ -436,7 +436,8 @@ class ControllerManager : public rclcpp::Node * lists match and returns a reference to it * This referenced list can be modified safely until switch_updated_controller_list() * is called, at this point the RT thread may start using it at any time - * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list + * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by + * rt list */ std::vector & get_unused_list( const std::lock_guard & guard); @@ -444,7 +445,8 @@ class ControllerManager : public rclcpp::Node /// get_updated_list Returns a const reference to the most updated list. /** * \warning May or may not being used by the realtime thread, read-only reference for safety - * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list + * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by + * rt list */ const std::vector & get_updated_list( const std::lock_guard & guard) const; @@ -452,7 +454,8 @@ class ControllerManager : public rclcpp::Node /** * switch_updated_list Switches the "updated" and "outdated" lists, and waits * until the RT thread is using the new "updated" list. - * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list + * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by + * rt list */ void switch_updated_list(const std::lock_guard & guard); diff --git a/controller_manager/package.xml b/controller_manager/package.xml index b64edb3135..c9dd319577 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.12.1 + 3.13.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 c2132cd2d0..0111b6f67e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -135,7 +135,8 @@ ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, const std::string & namespace_, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, namespace_, options), - resource_manager_(std::make_unique()), + resource_manager_(std::make_unique( + update_rate_, this->get_node_clock_interface())), diagnostics_updater_(this), executor_(executor), loader_(std::make_shared>( diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 9df237a9af..273e09d6a2 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -91,11 +91,17 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs const std::string & name, const std::string & type, const std::string & plugin_name, const uint8_t state_id, const std::string & state_label) { - EXPECT_EQ(component.name, name); - EXPECT_EQ(component.type, type); - EXPECT_EQ(component.plugin_name, plugin_name); - EXPECT_EQ(component.state.id, state_id); - EXPECT_EQ(component.state.label, state_label); + EXPECT_EQ(component.name, name) << "Component has unexpected name."; + EXPECT_EQ(component.type, type) + << "Component " << name << " from plugin " << plugin_name << " has wrong type."; + EXPECT_EQ(component.plugin_name, plugin_name) + << "Component " << name << " (" << type << ") has unexpected plugin_name."; + EXPECT_EQ(component.state.id, state_id) + << "Component " << name << " (" << type << ") from plugin " << plugin_name + << " has wrong state_id."; + EXPECT_EQ(component.state.label, state_label) + << "Component " << name << " (" << type << ") from plugin " << plugin_name + << " has wrong state_label."; } void list_hardware_components_and_check( @@ -124,8 +130,9 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs { auto it = std::find(interface_names.begin(), interface_names.end(), interfaces[i].name); EXPECT_NE(it, interface_names.end()); - EXPECT_EQ(interfaces[i].is_available, is_available_status[i]); - EXPECT_EQ(interfaces[i].is_claimed, is_claimed_status[i]); + EXPECT_EQ(interfaces[i].is_available, is_available_status[i]) + << "At " << interfaces[i].name; + EXPECT_EQ(interfaces[i].is_claimed, is_claimed_status[i]) << "At " << interfaces[i].name; } }; diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 5411ab39b5..66c0285a44 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + 3.12.1 (2023-04-14) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index ae70020730..38b24b6b26 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.12.1 + 3.13.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/doc/index.rst b/doc/index.rst index 9d614e1910..196ea93285 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -1,23 +1,29 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/index.rst + .. _ros2_control_framework: +################# +ros2_control +################# + +`GitHub Repository `_ + ================= API Documentation ================= API documentation is parsed by doxygen and can be found `here <../../api/index.html>`_ -===================== -Core functionalities -===================== -`GitHub Repository `_ +========= Features ========= -- :ref:`Command Line Interface (CLI) ` +* :ref:`Command Line Interface (CLI) ` +======== Concepts ======== diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index b819b8921b..245ae211e5 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.13.0 (2023-05-18) +------------------- +* Add class for thread management of async hw interfaces (`#981 `_) +* Fix github links on control.ros.org (`#1019 `_) +* Update precommit libraries(`#1020 `_) +* Implement parse_bool and refactor a few (`#1014 `_) +* docs: Fix link to hardware_components (`#1009 `_) +* Contributors: Alejandro Bordallo, Christoph Fröhlich, Felix Exner (fexner), Márk Szitanics, mosfet80 + +3.12.2 (2023-04-29) +------------------- + 3.12.1 (2023-04-14) ------------------- diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index 33230ffed1..cd93f472a5 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -1,10 +1,12 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/hardware_components_userdoc.rst + .. _hardware_components_userdoc: Hardware Components ------------------- Hardware components represent abstraction of physical hardware in ros2_control framework. There are three types of hardware Actuator, Sensor and System. -For details on each type check `Hardware Components description `_. +For details on each type check :ref:`overview_hardware_components` description. Guidelines and Best Practices ***************************** diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 0e74b9fb2d..7fd50fc9c8 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/mock_components_userdoc.rst + .. _mock_components_userdoc: Mock Components diff --git a/hardware_interface/doc/writing_new_hardware_interface.rst b/hardware_interface/doc/writing_new_hardware_interface.rst index fb748359eb..1ff4dc4420 100644 --- a/hardware_interface/doc/writing_new_hardware_interface.rst +++ b/hardware_interface/doc/writing_new_hardware_interface.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/writing_new_hardware_interface.rst + .. _writing_new_hardware_interface: Writing a new hardware interface @@ -28,7 +30,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 1. Take care that you use header guards. ROS2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ). 2. Include ``"hardware_interface/$interface_type$_interface.hpp"`` and ``visibility_control.h`` if you are using one. - ``$interface_type$`` can be ``Actuator``, ``Sensor`` or ``System`` depending on the type of hardware you are using. for more details about each type check `Hardware Components description `_. + ``$interface_type$`` can be ``Actuator``, ``Sensor`` or ``System`` depending on the type of hardware you are using. for more details about each type check :ref:`Hardware Components description `. 3. Define a unique namespace for your hardware_interface. This is usually the package name written in ``snake_case``. @@ -37,7 +39,7 @@ The following is a step-by-step guide to create source files, basic tests, and c class HardwareInterfaceName : public hardware_interface::$InterfaceType$Interface 5. Add a constructor without parameters and the following public methods implementing ``LifecycleNodeInterface``: ``on_configure``, ``on_cleanup``, ``on_shutdown``, ``on_activate``, ``on_deactivate``, ``on_error``; and overriding ``$InterfaceType$Interface`` definition: ``on_init``, ``export_state_interfaces``, ``export_command_interfaces``, ``prepare_command_mode_switch`` (optional), ``perform_command_mode_switch`` (optional), ``read``, ``write``. - For further explanation of hardware-lifecycle check the `pull request `_ and for exact definitions of methods check the ``"hardware_interface/$interface_type$_interface.hpp"`` header or `doxygen documentation `_ for *Actuator*, *Sensor* or *System*. + For further explanation of hardware-lifecycle check the `pull request `_ and for exact definitions of methods check the ``"hardware_interface/$interface_type$_interface.hpp"`` header or `doxygen documentation `_ for *Actuator*, *Sensor* or *System*. 4. **Adding definitions into source file (.cpp)** @@ -77,7 +79,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 5. **Writing export definition for pluginlib** 1. Create the ``.xml`` file in the package and add a definition of the library and hardware interface's class which has to be visible for the pluginlib. - The easiest way to do that is to check definition for mock components in the `hardware_interface mock_components `_ section. + The easiest way to do that is to check definition for mock components in the :ref:`hardware_interface mock_components ` section. 2. Usually, the plugin name is defined by the package (namespace) and the class name, e.g., ``/``. @@ -88,7 +90,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 1. Create the folder ``test`` in your package, if it does not exist already, and add a file named ``test_load_.cpp``. - 2. You can copy the ``load_generic_system_2dof`` content defined in the `test_generic_system.cpp `_ package. + 2. You can copy the ``load_generic_system_2dof`` content defined in the `test_generic_system.cpp `_ package. 3. Change the name of the copied test and in the last line, where hardware interface type is specified put the name defined in ``.xml`` file, e.g., ``/``. @@ -113,7 +115,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 7. In the test section add the following dependencies: ``ament_cmake_gmock``, ``hardware_interface``. 8. Add compile definitions for the tests using the ``ament_add_gmock`` directive. - For details, see how it is done for mock hardware in the `ros2_control `_ package. + For details, see how it is done for mock hardware in the `ros2_control `_ package. 9. (optional) Add your hardware interface`s library into ``ament_export_libraries`` before ``ament_package()``. @@ -137,4 +139,6 @@ That's it! Enjoy writing great controllers! Useful External References --------------------------- -- `Templates and scripts for generating controllers shell `_ **NOTE**: The script is currently only recommended to use for Foxy, not compatible with the API from Galactic and onwards. +- `Templates and scripts for generating controllers shell `_ + + .. NOTE:: The script is currently only recommended to use for Foxy, not compatible with the API from Galactic and onwards. diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 64c81b0a34..abfd8eb45a 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -35,8 +35,9 @@ namespace hardware_interface /** * The typical examples are conveyors or motors. * - * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - * with the following meaning: + * Methods return values have type + * rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn with the following + * meaning: * * \returns CallbackReturn::SUCCESS method execution was successful. * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. @@ -46,7 +47,8 @@ namespace hardware_interface * The hardware ends after each method in a state with the following meaning: * * UNCONFIGURED (on_init, on_cleanup): - * Hardware is initialized but communication is not started and therefore no interface is available. + * Hardware is initialized but communication is not started and therefore no interface is + * available. * * INACTIVE (on_configure, on_deactivate): * Communication with the hardware is started and it is configured. @@ -126,12 +128,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \note This is a non-realtime evaluation of whether a set of command interface claims are * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \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 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. + * \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 + * 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. */ virtual return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, @@ -145,11 +147,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * Perform the mode-switching for the new command interface combination. * * \note This is part of the realtime update loop, and should be fast. - * \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 stopping. - * \return return_type::OK if the new command interface combination can be switched to, + * \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 + * 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. */ virtual return_type perform_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/async_components.hpp b/hardware_interface/include/hardware_interface/async_components.hpp new file mode 100644 index 0000000000..2fa6fd7f85 --- /dev/null +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -0,0 +1,112 @@ +// Copyright 2023 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 HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ +#define HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ + +#include +#include +#include + +#include "hardware_interface/actuator.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/system.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/time.hpp" + +namespace hardware_interface +{ + +class AsyncComponentThread +{ +public: + explicit AsyncComponentThread( + Actuator * 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( + 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) + { + } + + AsyncComponentThread(const AsyncComponentThread & t) = delete; + AsyncComponentThread(AsyncComponentThread && t) = default; + + ~AsyncComponentThread() + { + terminated_.store(true, std::memory_order_seq_cst); + if (write_and_read_.joinable()) + { + write_and_read_.join(); + } + } + + void activate() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); } + + void write_and_read() + { + using TimePoint = std::chrono::system_clock::time_point; + + std::visit( + [this](auto & component) + { + auto previous_time = clock_interface_->get_clock()->now(); + while (!terminated_.load(std::memory_order_relaxed)) + { + auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_update_rate_); + TimePoint next_iteration_time = + TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds())); + + if (component->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + auto current_time = clock_interface_->get_clock()->now(); + auto measured_period = current_time - previous_time; + previous_time = current_time; + + // write + // read + } + next_iteration_time += period; + std::this_thread::sleep_until(next_iteration_time); + } + }, + hardware_component_); + } + +private: + std::atomic terminated_{false}; + std::variant hardware_component_; + std::thread write_and_read_{}; + + unsigned int cm_update_rate_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; +}; + +}; // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 1d0f07d94b..5112f7927e 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -26,12 +26,15 @@ namespace hardware_interface { /// Search XML snippet from URDF for information about a control component. /** - * \param[in] urdf string with robot's URDF - * \return vector filled with information about robot's control resources - * \throws std::runtime_error if a robot attribute or tag is not found - */ + * \param[in] urdf string with robot's URDF + * \return vector filled with information about robot's control resources + * \throws std::runtime_error if a robot attribute or tag is not found + */ HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); +HARDWARE_INTERFACE_PUBLIC +bool parse_bool(const std::string & bool_string); + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 836f12eb81..1afd78b582 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -16,25 +16,28 @@ #define HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_ #include -#include #include #include #include +#include "hardware_interface/actuator.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/system.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" #include "rclcpp/time.hpp" namespace hardware_interface { -class ActuatorInterface; -class SensorInterface; -class SystemInterface; class ResourceStorage; +class ControllerManager; struct HardwareReadWriteStatus { @@ -46,7 +49,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager { public: /// Default constructor for the Resource Manager. - ResourceManager(); + ResourceManager( + unsigned int update_rate = 100, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); /// Constructor for the Resource Manager. /** @@ -65,7 +70,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * "autostart_components" and "autoconfigure_components" instead. */ explicit ResourceManager( - const std::string & urdf, bool validate_interfaces = true, bool activate_all = false); + const std::string & urdf, bool validate_interfaces = true, bool activate_all = false, + unsigned int update_rate = 100, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); ResourceManager(const ResourceManager &) = delete; @@ -191,8 +198,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /** * Return list of cached controller names that use the hardware with name \p hardware_name. * - * \param[in] hardware_name the name of the hardware for which cached controllers should be returned. - * \returns list of cached controller names that depend on hardware with name \p hardware_name. + * \param[in] hardware_name the name of the hardware for which cached controllers should be + * returned. \returns list of cached controller names that depend on hardware with name \p + * hardware_name. */ std::vector get_cached_controllers_to_hardware(const std::string & hardware_name); diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index aaf9fcef8b..5d0677c587 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -77,6 +77,9 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); + HARDWARE_INTERFACE_PUBLIC + return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; } + private: std::unique_ptr impl_; }; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 2b2a4ce7cb..14a59e4588 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -35,8 +35,9 @@ namespace hardware_interface /** * The typical examples are Force-Torque Sensor (FTS), Interial Measurement Unit (IMU). * - * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - * with the following meaning: + * Methods return values have type + * rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn with the following + * meaning: * * \returns CallbackReturn::SUCCESS method execution was successful. * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. @@ -46,7 +47,8 @@ namespace hardware_interface * The hardware ends after each method in a state with the following meaning: * * UNCONFIGURED (on_init, on_cleanup): - * Hardware is initialized but communication is not started and therefore no interface is available. + * Hardware is initialized but communication is not started and therefore no interface is + * available. * * INACTIVE (on_configure, on_deactivate): * Communication with the hardware is started and it is configured. diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 75e1a1bc29..e5c6f2f542 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -36,8 +36,9 @@ namespace hardware_interface * The common examples for these types of hardware are multi-joint systems with or without sensors * such as industrial or humanoid robots. * - * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - * with the following meaning: + * Methods return values have type + * rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn with the following + * meaning: * * \returns CallbackReturn::SUCCESS method execution was successful. * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. @@ -47,7 +48,8 @@ namespace hardware_interface * The hardware ends after each method in a state with the following meaning: * * UNCONFIGURED (on_init, on_cleanup): - * Hardware is initialized but communication is not started and therefore no interface is available. + * Hardware is initialized but communication is not started and therefore no interface is + * available. * * INACTIVE (on_configure, on_deactivate): * Communication with the hardware is started and it is configured. @@ -127,12 +129,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \note This is a non-realtime evaluation of whether a set of command interface claims are * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \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 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. + * \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 + * 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. */ virtual return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, @@ -146,11 +148,11 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * Perform the mode-switching for the new command interface combination. * * \note This is part of the realtime update loop, and should be fast. - * \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 stopping. - * \return return_type::OK if the new command interface combination can be switched to, + * \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 + * 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. */ virtual return_type perform_command_mode_switch( diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 78ae1488a6..9506139c4a 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -81,12 +81,12 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector sensor_interfaces_; /// The size of this vector is (sensor_interfaces_.size() x nr_joints) - std::vector> sensor_fake_commands_; + std::vector> sensor_mock_commands_; std::vector> sensor_states_; std::vector gpio_interfaces_; /// The size of this vector is (gpio_interfaces_.size() x nr_joints) - std::vector> gpio_fake_commands_; + std::vector> gpio_mock_commands_; std::vector> gpio_commands_; std::vector> gpio_states_; @@ -108,7 +108,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector & interfaces, std::vector> & storage, std::vector & target_interfaces, bool using_state_interfaces); - bool use_fake_gpio_command_interfaces_; + bool use_mock_gpio_command_interfaces_; bool use_mock_sensor_command_interfaces_; double position_state_following_offset_; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index b257841b68..a024ad4a27 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.12.1 + 3.13.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 14f2dee21e..1338489a07 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -68,7 +69,8 @@ std::string get_text_for_element( const auto get_text_output = element_it->GetText(); if (!get_text_output) { - throw std::runtime_error("text not specified in the " + tag_name + " tag"); + std::cerr << "text not specified in the " << tag_name << " tag" << std::endl; + return ""; } return get_text_output; } @@ -224,7 +226,7 @@ std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem) bool parse_is_async_attribute(const tinyxml2::XMLElement * elem) { const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kIsAsyncAttribute); - return attr ? strcasecmp(attr->Value(), "true") == 0 : false; + return attr ? parse_bool(attr->Value()) : false; } /// Search XML snippet from URDF for parameters. @@ -302,11 +304,11 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( /// Search XML snippet from URDF for information about a control component. /** - * \param[in] component_it pointer to the iterator where component - * info should be found - * \return ComponentInfo filled with information about component - * \throws std::runtime_error if a component attribute or tag is not found - */ + * \param[in] component_it pointer to the iterator where component + * info should be found + * \return ComponentInfo filled with information about component + * \throws std::runtime_error if a component attribute or tag is not found + */ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it) { ComponentInfo component; @@ -415,10 +417,10 @@ ActuatorInfo parse_transmission_actuator_from_xml(const tinyxml2::XMLElement * e /// Search XML snippet from URDF for information about a transmission. /** - * \param[in] transmission_it pointer to the iterator where transmission info should be found - * \return TransmissionInfo filled with information about transmission - * \throws std::runtime_error if an attribute or tag is not found - */ + * \param[in] transmission_it pointer to the iterator where transmission info should be found + * \return TransmissionInfo filled with information about transmission + * \throws std::runtime_error if an attribute or tag is not found + */ TransmissionInfo parse_transmission_from_xml(const tinyxml2::XMLElement * transmission_it) { TransmissionInfo transmission; @@ -612,4 +614,9 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } +bool parse_bool(const std::string & bool_string) +{ + return bool_string == "true" || bool_string == "True"; +} + } // namespace hardware_interface diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index d1405dac8c..d490784dbd 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -25,6 +25,7 @@ #include #include +#include "hardware_interface/component_parser.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rcutils/logging_macros.h" @@ -74,8 +75,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i auto it = info_.hardware_parameters.find("mock_sensor_commands"); if (it != info_.hardware_parameters.end()) { - // TODO(anyone): change this to parse_bool() (see ros2_control#339) - use_mock_sensor_command_interfaces_ = it->second == "true" || it->second == "True"; + use_mock_sensor_command_interfaces_ = hardware_interface::parse_bool(it->second); } else { @@ -83,9 +83,9 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i it = info_.hardware_parameters.find("fake_sensor_commands"); if (it != info_.hardware_parameters.end()) { - use_mock_sensor_command_interfaces_ = it->second == "true" || it->second == "True"; + use_mock_sensor_command_interfaces_ = hardware_interface::parse_bool(it->second); RCUTILS_LOG_WARN_NAMED( - "fake_generic_system", + "mock_generic_system", "Parameter 'fake_sensor_commands' has been deprecated from usage. Use" "'mock_sensor_commands' instead."); } @@ -95,16 +95,28 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } - // check if to create fake command interface for gpio - it = info_.hardware_parameters.find("fake_gpio_commands"); + // check if to create mock command interface for gpio + it = info_.hardware_parameters.find("mock_gpio_commands"); if (it != info_.hardware_parameters.end()) { - // TODO(anyone): change this to parse_bool() (see ros2_control#339) - use_fake_gpio_command_interfaces_ = it->second == "true" || it->second == "True"; + use_mock_gpio_command_interfaces_ = hardware_interface::parse_bool(it->second); } else { - use_fake_gpio_command_interfaces_ = false; + // check if fake_gpio_commands was set instead and issue warning + it = info_.hardware_parameters.find("fake_gpio_commands"); + if (it != info_.hardware_parameters.end()) + { + use_mock_gpio_command_interfaces_ = hardware_interface::parse_bool(it->second); + RCUTILS_LOG_WARN_NAMED( + "mock_generic_system", + "Parameter 'fake_gpio_commands' has been deprecated from usage. Use" + "'mock_gpio_commands' instead."); + } + else + { + use_mock_gpio_command_interfaces_ = false; + } } // process parameters about state following @@ -121,7 +133,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i custom_interface_with_following_offset_ = it->second; } } - // its extremlly unprobably that std::distance results int this value - therefore default + // it's extremely improbable that std::distance results int this value - therefore default index_custom_interface_with_following_offset_ = std::numeric_limits::max(); // Initialize storage for standard interfaces @@ -189,14 +201,14 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i index_custom_interface_with_following_offset_ = std::distance(other_interfaces_.begin(), if_it); RCUTILS_LOG_INFO_NAMED( - "fake_generic_system", "Custom interface with following offset '%s' found at index: %zu.", + "mock_generic_system", "Custom interface with following offset '%s' found at index: %zu.", custom_interface_with_following_offset_.c_str(), index_custom_interface_with_following_offset_); } else { RCUTILS_LOG_WARN_NAMED( - "fake_generic_system", + "mock_generic_system", "Custom interface with following offset '%s' does not exist. Offset will not be applied", custom_interface_with_following_offset_.c_str()); } @@ -215,7 +227,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } initialize_storage_vectors( - sensor_fake_commands_, sensor_states_, sensor_interfaces_, info_.sensors); + sensor_mock_commands_, sensor_states_, sensor_interfaces_, info_.sensors); // search for gpio interfaces for (const auto & gpio : info_.gpios) @@ -227,10 +239,10 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i populate_non_standard_interfaces(gpio.state_interfaces, gpio_interfaces_); } - // Fake gpio command interfaces - if (use_fake_gpio_command_interfaces_) + // Mock gpio command interfaces + if (use_mock_gpio_command_interfaces_) { - initialize_storage_vectors(gpio_fake_commands_, gpio_states_, gpio_interfaces_, info_.gpios); + initialize_storage_vectors(gpio_mock_commands_, gpio_states_, gpio_interfaces_, info_.gpios); } // Real gpio command interfaces else @@ -310,22 +322,22 @@ std::vector GenericSystem::export_command_ } } - // Fake sensor command interfaces + // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) { if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_fake_commands_, command_interfaces, true)) + info_.sensors, sensor_interfaces_, sensor_mock_commands_, command_interfaces, true)) { throw std::runtime_error( "Interface is not found in the standard nor other list. This should never happen!"); } } - // Fake gpio command interfaces (consider all state interfaces for command interfaces) - if (use_fake_gpio_command_interfaces_) + // Mock gpio command interfaces (consider all state interfaces for command interfaces) + if (use_mock_gpio_command_interfaces_) { if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_fake_commands_, command_interfaces, true)) + info_.gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, true)) { throw std::runtime_error( "Interface is not found in the gpio list. This should never happen!"); @@ -404,13 +416,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur if (use_mock_sensor_command_interfaces_) { - mirror_command_to_state(sensor_states_, sensor_fake_commands_); + mirror_command_to_state(sensor_states_, sensor_mock_commands_); } // do loopback on all gpio interfaces - if (use_fake_gpio_command_interfaces_) + if (use_mock_gpio_command_interfaces_) { - mirror_command_to_state(gpio_states_, gpio_fake_commands_); + mirror_command_to_state(gpio_states_, gpio_mock_commands_); } else { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 42285a2839..b24e812b07 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -17,13 +17,16 @@ #include #include #include +#include #include +#include #include #include #include #include "hardware_interface/actuator.hpp" #include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/async_components.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/sensor.hpp" @@ -73,10 +76,16 @@ class ResourceStorage static constexpr const char * system_interface_name = "hardware_interface::SystemInterface"; public: - ResourceStorage() + // TODO(VX792): Change this when HW ifs get their own update rate, + // because the ResourceStorage really shouldn't know about the cm's parameters + ResourceStorage( + unsigned int update_rate = 100, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr) : actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), - system_loader_(pkg_name, system_interface_name) + system_loader_(pkg_name, system_interface_name), + cm_update_rate_(update_rate), + clock_interface_(clock_interface) { } @@ -192,6 +201,13 @@ class ResourceStorage hardware.get_name().c_str(), interface.c_str()); } } + + if (hardware_info_map_[hardware.get_name()].is_async) + { + async_component_threads_.emplace( + std::piecewise_construct, std::forward_as_tuple(hardware.get_name()), + std::forward_as_tuple(&hardware, cm_update_rate_, clock_interface_)); + } } return result; } @@ -271,6 +287,7 @@ class ResourceStorage if (result) { + async_component_threads_.erase(hardware.get_name()); // TODO(destogl): change this - deimport all things if there is there are interfaces there // deimport_non_movement_command_interfaces(hardware); // deimport_state_interfaces(hardware); @@ -288,6 +305,10 @@ class ResourceStorage if (result) { + if (async_component_threads_.find(hardware.get_name()) != async_component_threads_.end()) + { + async_component_threads_.at(hardware.get_name()).activate(); + } // TODO(destogl): make all command interfaces available (currently are all available) } @@ -489,54 +510,126 @@ class ResourceStorage // TODO(destogl): Propagate "false" up, if happens in initialize_hardware void load_and_initialize_actuator(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, actuator_loader_, actuators_); - initialize_hardware(hardware_info, actuators_.back()); - import_state_interfaces(actuators_.back()); - import_command_interfaces(actuators_.back()); + auto load_and_init_actuators = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, actuator_loader_, container); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + load_and_init_actuators(async_actuators_); + } + else + { + load_and_init_actuators(actuators_); + } } void load_and_initialize_sensor(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, sensor_loader_, sensors_); - initialize_hardware(hardware_info, sensors_.back()); - import_state_interfaces(sensors_.back()); + auto load_and_init_sensors = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, sensor_loader_, container); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + load_and_init_sensors(async_sensors_); + } + else + { + load_and_init_sensors(sensors_); + } } void load_and_initialize_system(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, system_loader_, systems_); - initialize_hardware(hardware_info, systems_.back()); - import_state_interfaces(systems_.back()); - import_command_interfaces(systems_.back()); + auto load_and_init_systems = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, system_loader_, container); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + load_and_init_systems(async_systems_); + } + else + { + load_and_init_systems(systems_); + } } void initialize_actuator( std::unique_ptr actuator, const HardwareInfo & hardware_info) { - this->actuators_.emplace_back(Actuator(std::move(actuator))); - initialize_hardware(hardware_info, actuators_.back()); - import_state_interfaces(actuators_.back()); - import_command_interfaces(actuators_.back()); + auto init_actuators = [&](auto & container) + { + container.emplace_back(Actuator(std::move(actuator))); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + init_actuators(async_actuators_); + } + else + { + init_actuators(actuators_); + } } void initialize_sensor( std::unique_ptr sensor, const HardwareInfo & hardware_info) { - this->sensors_.emplace_back(Sensor(std::move(sensor))); - initialize_hardware(hardware_info, sensors_.back()); - import_state_interfaces(sensors_.back()); + auto init_sensors = [&](auto & container) + { + container.emplace_back(Sensor(std::move(sensor))); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + init_sensors(async_sensors_); + } + else + { + init_sensors(sensors_); + } } void initialize_system( std::unique_ptr system, const HardwareInfo & hardware_info) { - this->systems_.emplace_back(System(std::move(system))); - initialize_hardware(hardware_info, systems_.back()); - import_state_interfaces(systems_.back()); - import_command_interfaces(systems_.back()); + auto init_systems = [&](auto & container) + { + container.emplace_back(System(std::move(system))); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + init_systems(async_systems_); + } + else + { + init_systems(systems_); + } } // hardware plugins @@ -548,6 +641,10 @@ class ResourceStorage std::vector sensors_; std::vector systems_; + std::vector async_actuators_; + std::vector async_sensors_; + std::vector async_systems_; + std::unordered_map hardware_info_map_; /// Mapping between hardware and controllers that are using it (accessing data from it) @@ -567,15 +664,28 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; + + /// List of async components by type + std::unordered_map async_component_threads_; + + // Update rate of the controller manager, and the clock interface of its node + // Used by async components. + unsigned int cm_update_rate_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; }; -ResourceManager::ResourceManager() : resource_storage_(std::make_unique()) {} +ResourceManager::ResourceManager( + unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +: resource_storage_(std::make_unique(update_rate, clock_interface)) +{ +} ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( - const std::string & urdf, bool validate_interfaces, bool activate_all) -: resource_storage_(std::make_unique()) + const std::string & urdf, bool validate_interfaces, bool activate_all, unsigned int update_rate, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +: resource_storage_(std::make_unique(update_rate, clock_interface)) { load_urdf(urdf, validate_interfaces); @@ -889,18 +999,20 @@ void ResourceManager::import_component( // CM API: Called in "callback/slow"-thread std::unordered_map ResourceManager::get_components_status() { - for (auto & component : resource_storage_->actuators_) - { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); - } - for (auto & component : resource_storage_->sensors_) + auto loop_and_get_state = [&](auto & container) { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); - } - for (auto & component : resource_storage_->systems_) - { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); - } + for (auto & component : container) + { + resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); + } + }; + + loop_and_get_state(resource_storage_->actuators_); + loop_and_get_state(resource_storage_->async_actuators_); + loop_and_get_state(resource_storage_->sensors_); + loop_and_get_state(resource_storage_->async_sensors_); + loop_and_get_state(resource_storage_->systems_); + loop_and_get_state(resource_storage_->async_systems_); return resource_storage_->hardware_info_map_; } @@ -1059,6 +1171,24 @@ return_type ResourceManager::set_component_state( std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), resource_storage_->systems_); } + if (!found) + { + found = find_set_component_state( + std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), + resource_storage_->async_actuators_); + } + if (!found) + { + found = find_set_component_state( + std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), + resource_storage_->async_systems_); + } + if (!found) + { + found = find_set_component_state( + std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), + resource_storage_->async_sensors_); + } return result; } @@ -1147,6 +1277,7 @@ bool ResourceManager::state_interface_exists(const std::string & key) const return resource_storage_->state_interface_map_.find(key) != resource_storage_->state_interface_map_.end(); } + // END: "used only in tests and locally" // BEGIN: private methods diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 9fb1aa4988..e82d7d1fb2 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -39,8 +39,9 @@ const auto PERIOD = rclcpp::Duration::from_seconds(0.01); class TestGenericSystem : public ::testing::Test { public: - void test_generic_system_with_mock_sensor_commands(std::string & urdf); void test_generic_system_with_mimic_joint(std::string & urdf); + void test_generic_system_with_mock_sensor_commands(std::string & urdf); + void test_generic_system_with_mock_gpio_commands(std::string & urdf); protected: void SetUp() override @@ -185,7 +186,7 @@ class TestGenericSystem : public ::testing::Test )"; - hardware_system_2dof_with_sensor_fake_command_ = + hardware_system_2dof_with_sensor_mock_command_ = R"( @@ -214,7 +215,7 @@ class TestGenericSystem : public ::testing::Test )"; - hardware_system_2dof_with_sensor_fake_command_True_ = + hardware_system_2dof_with_sensor_mock_command_True_ = R"( @@ -381,7 +382,41 @@ class TestGenericSystem : public ::testing::Test )"; - valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ = + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ = + R"( + + + mock_components/GenericSystem + true + + + + + + + 3.45 + + + + + + + 2.78 + + + + + + + + + + + + +)"; + + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ = R"( @@ -457,14 +492,15 @@ class TestGenericSystem : public ::testing::Test std::string hardware_system_2dof_standard_interfaces_; std::string hardware_system_2dof_with_other_interface_; std::string hardware_system_2dof_with_sensor_; - std::string hardware_system_2dof_with_sensor_fake_command_; - std::string hardware_system_2dof_with_sensor_fake_command_True_; + std::string hardware_system_2dof_with_sensor_mock_command_; + std::string hardware_system_2dof_with_sensor_mock_command_True_; std::string hardware_system_2dof_with_mimic_joint_; std::string hardware_system_2dof_standard_interfaces_with_offset_; std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_; std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_; std::string valid_urdf_ros2_control_system_robot_with_gpio_; - std::string valid_urdf_ros2_control_system_robot_with_gpio_fake_command_; + std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_; + std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_; std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; }; @@ -485,12 +521,12 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(TestGenericSystem, generic_system_2dof_asymetric_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_other_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command); - FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command_True); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command_True); FRIEND_TEST(TestGenericSystem, hardware_system_2dof_with_mimic_joint); FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio); - FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command); - FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command); + FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command); + FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True); TestableResourceManager() : hardware_interface::ResourceManager() {} @@ -1070,18 +1106,18 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands(std::strin ASSERT_EQ(4.44, sty_c.get_value()); } -TEST_F(TestGenericSystem, generic_system_2dof_sensor_fake_command) +TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command) { - auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_fake_command_ + + auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_mock_command_ + ros2_control_test_assets::urdf_tail; test_generic_system_with_mock_sensor_commands(urdf); } -TEST_F(TestGenericSystem, generic_system_2dof_sensor_fake_command_True) +TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) { auto urdf = ros2_control_test_assets::urdf_head + - hardware_system_2dof_with_sensor_fake_command_True_ + + hardware_system_2dof_with_sensor_mock_command_True_ + ros2_control_test_assets::urdf_tail; test_generic_system_with_mock_sensor_commands(urdf); @@ -1396,11 +1432,8 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) generic_system_functional_test(urdf); } -TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command) +void TestGenericSystem::test_generic_system_with_mock_gpio_commands(std::string & urdf) { - auto urdf = ros2_control_test_assets::urdf_head + - valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ + - ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // check is hardware is started @@ -1507,7 +1540,25 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_co ASSERT_EQ(2.22, gpio2_vac_c.get_value()); } -TEST_F(TestGenericSystem, sensor_with_initial_value_) +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command) +{ + auto urdf = ros2_control_test_assets::urdf_head + + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ + + ros2_control_test_assets::urdf_tail; + + test_generic_system_with_mock_gpio_commands(urdf); +} + +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True) +{ + auto urdf = ros2_control_test_assets::urdf_head + + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ + + ros2_control_test_assets::urdf_tail; + + test_generic_system_with_mock_gpio_commands(urdf); +} + +TEST_F(TestGenericSystem, sensor_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + sensor_with_initial_value_ + ros2_control_test_assets::urdf_tail; @@ -1535,7 +1586,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value_) ASSERT_EQ(0.0, force_z_s.get_value()); } -TEST_F(TestGenericSystem, gpio_with_initial_value_) +TEST_F(TestGenericSystem, gpio_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + gpio_with_initial_value_ + ros2_control_test_assets::urdf_tail; diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 97d4a77d21..b0c7c5a16d 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -100,16 +100,6 @@ TEST_F(TestComponentParser, component_interface_type_empty_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); } -TEST_F(TestComponentParser, parameter_empty_throws_error) -{ - const std::string broken_urdf_string = - std::string(ros2_control_test_assets::urdf_head) + - ros2_control_test_assets::invalid_urdf_ros2_control_parameter_empty + - ros2_control_test_assets::urdf_tail; - - ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); -} - TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) { std::string urdf_to_test = @@ -625,6 +615,32 @@ 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_parameter_empty) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_parameter_empty + + 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, "2DOF_System_Robot_Position_Only"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, + "ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only"); + + ASSERT_THAT(hardware_info.joints, SizeIs(1)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "position"); + + 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"); +} + TEST_F(TestComponentParser, negative_size_throws_error) { std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 8fec418dcf..5f9c09e95e 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -40,7 +40,7 @@ class TestActuator : public ActuatorInterface * if (info_.joints[0].command_interfaces.size() != 1) {return CallbackReturn::ERROR;} * // can only give feedback state for position and velocity * if (info_.joints[0].state_interfaces.size() != 2) {return CallbackReturn::ERROR;} - */ + */ return CallbackReturn::SUCCESS; } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index f7d1962316..c1c054713c 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -368,6 +368,7 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) hardware_interface::HardwareInfo external_component_hw_info; external_component_hw_info.name = "ExternalComponent"; external_component_hw_info.type = "actuator"; + external_component_hw_info.is_async = false; rm.import_component(std::make_unique(), external_component_hw_info); EXPECT_EQ(2u, rm.actuator_components_size()); diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 9180ad3295..211828a28b 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + 3.12.1 (2023-04-14) ------------------- diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 5d1fd6a1f7..2f32d49271 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -28,11 +28,11 @@ namespace // utilities { /// Declare and initialize a parameter with a type. /** - * - * Wrapper function for templated node's declare_parameter() which checks if - * parameter is already declared. - * For use in all components that inherit from ControllerInterface - */ + * + * Wrapper function for templated node's declare_parameter() which checks if + * parameter is already declared. + * For use in all components that inherit from ControllerInterface + */ template auto auto_declare( const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, @@ -149,10 +149,9 @@ inline bool declare_parameters(const std::string & joint_name, const rclcpp::Nod } /** - * Declare JointLimits and SoftJointLimits parameters for joint with joint_name for the lifecycle_node - * object. - * This is a convenience function. - * For parameters structure see the underlying `declare_parameters` function. + * Declare JointLimits and SoftJointLimits parameters for joint with joint_name for the + * lifecycle_node object. This is a convenience function. For parameters structure see the + * underlying `declare_parameters` function. * * \param[in] joint_name name of the joint for which parameters will be declared. * \param[in] lifecycle_node lifecycle node for parameters should be declared. @@ -217,10 +216,12 @@ inline bool declare_parameters( * \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint". * \param[in] param_itf node parameters interface of the node where parameters are specified. * \param[in] logging_itf node logging interface to provide log errors. - * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter + * server will overwrite existing values. Values in \p limits not specified in the parameter server + * remain unchanged. * - * \returns True if a limits specification is found (i.e., the \p joint_limits/joint_name parameter exists in \p node), false otherwise. + * \returns True if a limits specification is found (i.e., the \p joint_limits/joint_name parameter + * exists in \p node), false otherwise. */ inline bool get_joint_limits( const std::string & joint_name, @@ -385,8 +386,9 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] node Node object for which parameters should be fetched. - * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter + * server will overwrite existing values. Values in \p limits not specified in the parameter server + * remain unchanged. * * \returns True if a limits specification is found, false otherwise. */ @@ -404,8 +406,9 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched. - * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter + * server will overwrite existing values. Values in \p limits not specified in the parameter server + * remain unchanged. * * \returns True if a limits specification is found, false otherwise. */ @@ -445,10 +448,10 @@ inline bool get_joint_limits( * \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint". * \param[in] param_itf node parameters interface of the node where parameters are specified. * \param[in] logging_itf node logging interface to provide log errors. - * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. - * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and - * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the + * parameter server will overwrite existing values. \return True if a complete soft limits + * specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and \p + * soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. */ inline bool get_joint_limits( const std::string & joint_name, @@ -512,8 +515,8 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] node Node object for which parameters should be fetched. - * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the + * parameter server will overwrite existing values. * * \returns True if a soft limits specification is found, false otherwise. */ @@ -533,8 +536,8 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched. - * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the + * parameter server will overwrite existing values. * * \returns True if a soft limits specification is found, false otherwise. */ diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 081478e904..7bcf465d24 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.12.1 + 3.13.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp index 3f17ceea78..db553948d1 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -144,8 +144,8 @@ class JointSoftLimitsHandle : public JointLimitHandle joint_limits_interface::SoftJointLimits soft_limits_; }; -/** A handle used to enforce position and velocity limits of a position-controlled joint that does not have - soft limits. */ +/** A handle used to enforce position and velocity limits of a position-controlled joint that does + not have soft limits. */ class PositionJointSaturationHandle : public JointLimitHandle { public: @@ -170,8 +170,8 @@ class PositionJointSaturationHandle : public JointLimitHandle /// Enforce position and velocity limits for a joint that is not subject to soft limits. /** - * \param[in] period Control period. - */ + * \param[in] period Control period. + */ void enforce_limits(const rclcpp::Duration & period) { if (std::isnan(prev_pos_)) @@ -209,9 +209,9 @@ class PositionJointSaturationHandle : public JointLimitHandle /// A handle used to enforce position and velocity limits of a position-controlled joint. /** - * This class implements a very simple position and velocity limits enforcing policy, and tries to impose the least - * amount of requisites on the underlying hardware platform. - * This lowers considerably the entry barrier to use it, but also implies some limitations. + * This class implements a very simple position and velocity limits enforcing policy, and tries to + * impose the least amount of requisites on the underlying hardware platform. This lowers + * considerably the entry barrier to use it, but also implies some limitations. * * Requisites * - Position (for non-continuous joints) and velocity limits specification. @@ -219,21 +219,22 @@ class PositionJointSaturationHandle : public JointLimitHandle * * Open loop nature * - * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is checked for - * validity without relying on the actual position/velocity values. + * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is + * checked for validity without relying on the actual position/velocity values. * - * - Actual position values are \e not used because in some platforms there might be a substantial lag - * between sending a command and executing it (propagate command to hardware, reach control objective, - * read from hardware). + * - Actual position values are \e not used because in some platforms there might be a substantial + * lag between sending a command and executing it (propagate command to hardware, reach control + * objective, read from hardware). * - * - Actual velocity values are \e not used because of the above reason, and because some platforms might not expose - * trustworthy velocity measurements, or none at all. + * - Actual velocity values are \e not used because of the above reason, and because some platforms + * might not expose trustworthy velocity measurements, or none at all. * - * The downside of the open loop behavior is that velocity limits will not be enforced when recovering from large - * position tracking errors. Only the command is guaranteed to comply with the limits specification. + * The downside of the open loop behavior is that velocity limits will not be enforced when + * recovering from large position tracking errors. Only the command is guaranteed to comply with the + * limits specification. * - * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate the command - * velocity. + * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate + * the command velocity. */ // TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? @@ -553,8 +554,8 @@ class VelocityJointSaturationHandle : public JointLimitHandle /** * A handle used to enforce position, velocity, and acceleration limits of a - * velocity-controlled joint. - */ + * velocity-controlled joint. + */ class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle { public: diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp index 6f26a997ee..80cf7f0ed4 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp @@ -27,8 +27,8 @@ namespace joint_limits_interface /** * Populate a JointLimits instance from URDF joint data. * \param[in] urdf_joint URDF joint. - * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will overwrite existing - * values. Values in \e limits not present in \e urdf_joint remain unchanged. + * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will + * overwrite existing values. Values in \e limits not present in \e urdf_joint remain unchanged. * \return True if \e urdf_joint has a valid limits specification, false otherwise. */ inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 2e1c00c372..d6e3effbb0 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + 3.12.1 (2023-04-14) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 601ed4bdc8..ddc9ae5edd 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.12.1 + 3.13.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 892186bd89..e5b824a921 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + 3.12.1 (2023-04-14) ------------------- diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index f3e2bda2c0..a42d39a241 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -398,6 +398,21 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type = )"; +const auto valid_urdf_ros2_control_parameter_empty = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + + 2 + + + + + + +)"; + // Errors const auto invalid_urdf_ros2_control_invalid_child = R"( @@ -485,23 +500,6 @@ const auto invalid_urdf_ros2_control_component_interface_type_empty = )"; -const auto invalid_urdf_ros2_control_parameter_empty = - R"( - - - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only - - 2 - - - - -1 - 1 - - - -)"; - const auto invalid_urdf2_ros2_control_illegal_size = R"( diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 568ce07a65..00c489b6a2 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.12.1 + 3.13.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 73687b1dd2..0af749a91a 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.13.0 (2023-05-18) +------------------- +* Fix github links on control.ros.org (`#1019 `_) +* Contributors: Christoph Fröhlich + +3.12.2 (2023-04-29) +------------------- +* Fix verbose output of list_hardware_components (`#1004 `_) +* Contributors: Christoph Fröhlich + 3.12.1 (2023-04-14) ------------------- diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 6ae26df325..dba173cfbf 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -1,3 +1,5 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/ros2controlcli/doc/userdoc.rst + .. _ros2controlcli_userdoc: Command Line Interface diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 24773e9992..ca01cbe271 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.12.1 + 3.13.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index 2f94d70969..54dff21bb6 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -65,17 +65,12 @@ def main(self, *, args): if args.verbose: print("\tstate interfaces") - for state_interface in component.command_interfaces: + for state_interface in component.state_interfaces: if state_interface.is_available: available_str = f"{bcolors.OKBLUE}[available]{bcolors.ENDC}" else: available_str = f"{bcolors.WARNING}[unavailable]{bcolors.ENDC}" - if state_interface.is_claimed: - claimed_str = f"{bcolors.OKBLUE}[claimed]{bcolors.ENDC}" - else: - claimed_str = "[unclaimed]" - - print(f"\t\t{state_interface.name} {available_str} {claimed_str}") + print(f"\t\t{state_interface.name} {available_str}") return 0 diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 9455ffc0d7..b916f1a2af 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.12.1", + version="3.13.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index ab6544a27f..f57aa7fcbd 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + 3.12.1 (2023-04-14) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 07571ab6e2..acd6496e8d 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.12.1 + 3.13.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 7e9127b209..7d12c68e1f 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.12.1", + version="3.13.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 6ec67d7349..7dded27cc1 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.13.0 (2023-05-18) +------------------- + +3.12.2 (2023-04-29) +------------------- + 3.12.1 (2023-04-14) ------------------- diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 6bc89ab893..2002f6f9d7 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -30,9 +30,8 @@ namespace transmission_interface /// Implementation of a differential transmission. /** * - * This transmission relates two actuators and two joints through a differential mechanism, as illustrated - * below. - * \image html differential_transmission.png + * This transmission relates two actuators and two joints through a differential + * mechanism, as illustrated below. \image html differential_transmission.png * *
* @@ -76,28 +75,33 @@ namespace transmission_interface * *
* \f{eqnarray*}{ - * x_{a_1} & = & n_{a_1} \left[ n_{j_1} (x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2}) \right] \\[2.5em] - * x_{a_2} & = & n_{a_2} \left[ n_{j_1} (x_{j_1} - x_{off_1}) - n_{j_2} (x_{j_2} - x_{off_2}) \right] - * \f} + * x_{a_1} & = & n_{a_1} \left[ n_{j_1} (x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2}) + * \right] \\[2.5em] x_{a_2} & = & n_{a_2} \left[ n_{j_1} (x_{j_1} - x_{off_1}) - n_{j_2} (x_{j_2} - + * x_{off_2}) \right] \f} *
*
* * where: - * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively. - * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively. - * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates - * (cf. SimpleTransmission class documentation for a more detailed description of this variable). - * - \f$ n \f$ represents a transmission ratio. Reducers/amplifiers are allowed on both the actuator and joint sides - * (depicted as timing belts in the figure). - * A transmission ratio can take any real value \e except zero. In particular: - * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute - * value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. - * - Negative values represent a direction flip, ie. input and output move in opposite directions. - * - Important: Use transmission ratio signs to match this class' convention of positive actuator/joint - * directions with a given mechanical design, as they will in general not match. + * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, + * respectively. + * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space + * variables, respectively. + * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position + * coordinates (cf. SimpleTransmission class documentation for a more detailed description of this + * variable). + * - \f$ n \f$ represents a transmission ratio. Reducers/amplifiers are allowed on both the actuator + * and joint sides (depicted as timing belts in the figure). A transmission ratio can take any real + * value \e except zero. In particular: + * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, + * while if its absolute value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. + * - Negative values represent a direction flip, ie. input and output move in opposite + * directions. + * - Important: Use transmission ratio signs to match this class' convention of positive + * actuator/joint directions with a given mechanical design, as they will in general not match. * - * \note This implementation currently assumes a specific layout for location of the actuators and joint axes which is - * common in robotic mechanisms. Please file an enhancement ticket if your use case does not adhere to this layout. + * \note This implementation currently assumes a specific layout for location of the actuators and + * joint axes which is common in robotic mechanisms. Please file an enhancement ticket if your use + * case does not adhere to this layout. * * \ingroup transmission_types */ @@ -126,14 +130,16 @@ class DifferentialTransmission : public Transmission /// Transform variables from actuator to joint space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void actuator_to_joint() override; /// Transform variables from joint to actuator space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void joint_to_actuator() override; diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index b8eb05e8d3..39f5df6f61 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -29,77 +29,80 @@ namespace transmission_interface { /// Implementation of a four-bar-linkage transmission. /** -* -* This transmission relates two actuators and two joints through a mechanism in which the state of the -* first joint only depends on the first actuator, while the second joint depends on both actuators, as -* illustrated below. -* Although the class name makes specific reference to the four-bar-linkage, there are other mechanical layouts -* that yield the same behavior, such as the remote actuation example also depicted below. -* \image html four_bar_linkage_transmission.png -* -*
-* -* -* -* -* -* -* -* -* -* -*
Effort
Velocity
Position
-* Actuator to joint -* -* \f{eqnarray*}{ -* \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\ -* \tau_{j_2} & = & n_{j_2} (n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1}) -* \f} -* -* \f{eqnarray*}{ -* \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} }{ n_{j_1} n_{a_1} } \\ -* \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_2} / n_{a_2} - \dot{x}_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } -* \f} -* -* \f{eqnarray*}{ -* x_{j_1} & = & \frac{ x_{a_1} }{ n_{j_1} n_{a_1} } + x_{off_1} \\ -* x_{j_2} & = & \frac{ x_{a_2} / n_{a_2} - x_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } + x_{off_2} -* \f} -*
-* Joint to actuator -* -* \f{eqnarray*}{ -* \tau_{a_1} & = & \tau_{j_1} / (n_{j_1} n_{a_1}) \\ -* \tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} } -* \f} -* -* \f{eqnarray*}{ -* \dot{x}_{a_1} & = & n_{j_1} n_{a_1} \dot{x}_{j_1} \\ -* \dot{x}_{a_2} & = & n_{a_2} (\dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2}) -* \f} -* -* \f{eqnarray*}{ -* x_{a_1} & = & n_{j_1} n_{a_1} (x_{j_1} - x_{off_1}) \\ -* x_{a_2} & = & n_{a_2} \left[(x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2})\right] -* \f} -*
-*
-* -* where: -* - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively. -* - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively. -* - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates. -* (cf. SimpleTransmission class documentation for a more detailed description of this variable). -* - \f$ n \f$ represents a transmission ratio (reducers/amplifiers are depicted as timing belts in the figure). -* A transmission ratio can take any real value \e except zero. In particular: -* - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute -* value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. -* - Negative values represent a direction flip, ie. input and output move in opposite directions. -* - Important: Use transmission ratio signs to match this class' convention of positive actuator/joint -* directions with a given mechanical design, as they will in general not match. -* -* \ingroup transmission_types -*/ + * + * This transmission relates two actuators and two joints through a mechanism in which + * the state of the first joint only depends on the first actuator, while the second joint depends + * on both actuators, as illustrated below. Although the class name makes specific reference to the + * four-bar-linkage, there are other mechanical layouts that yield the same behavior, such as the + * remote actuation example also depicted below. \image html four_bar_linkage_transmission.png + * + *
+ * + * + * + * + * + * + * + * + * + * + *
Effort
Velocity
Position
+ * Actuator to joint + * + * \f{eqnarray*}{ + * \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\ + * \tau_{j_2} & = & n_{j_2} (n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1}) + * \f} + * + * \f{eqnarray*}{ + * \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} }{ n_{j_1} n_{a_1} } \\ + * \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_2} / n_{a_2} - \dot{x}_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} + * } \f} + * + * \f{eqnarray*}{ + * x_{j_1} & = & \frac{ x_{a_1} }{ n_{j_1} n_{a_1} } + x_{off_1} \\ + * x_{j_2} & = & \frac{ x_{a_2} / n_{a_2} - x_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } + x_{off_2} + * \f} + *
+ * Joint to actuator + * + * \f{eqnarray*}{ + * \tau_{a_1} & = & \tau_{j_1} / (n_{j_1} n_{a_1}) \\ + * \tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} } + * \f} + * + * \f{eqnarray*}{ + * \dot{x}_{a_1} & = & n_{j_1} n_{a_1} \dot{x}_{j_1} \\ + * \dot{x}_{a_2} & = & n_{a_2} (\dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2}) + * \f} + * + * \f{eqnarray*}{ + * x_{a_1} & = & n_{j_1} n_{a_1} (x_{j_1} - x_{off_1}) \\ + * x_{a_2} & = & n_{a_2} \left[(x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2})\right] + * \f} + *
+ *
+ * + * where: + * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, + * respectively. + * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space + * variables, respectively. + * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position + * coordinates. (cf. SimpleTransmission class documentation for a more detailed description of this + * variable). + * - \f$ n \f$ represents a transmission ratio (reducers/amplifiers are depicted as timing belts in + * the figure). A transmission ratio can take any real value \e except zero. In particular: + * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, + * while if its absolute value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. + * - Negative values represent a direction flip, ie. input and output move in opposite + * directions. + * - Important: Use transmission ratio signs to match this class' convention of positive + * actuator/joint directions with a given mechanical design, as they will in general not match. + * + * \ingroup transmission_types + */ class FourBarLinkageTransmission : public Transmission { public: @@ -125,14 +128,16 @@ class FourBarLinkageTransmission : public Transmission /// Transform variables from actuator to joint space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void actuator_to_joint() override; /// Transform variables from joint to actuator space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void joint_to_actuator() override; diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index a74cc3dddd..809472ab75 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -28,9 +28,9 @@ namespace transmission_interface { /// Implementation of a simple reducer transmission. /** - * This transmission relates one actuator and one joint through a reductor (or amplifier). - * Timing belts and gears are examples of this transmission type, and are illustrated below. - * \image html simple_transmission.png + * This transmission relates one actuator and one joint through a reductor (or + * amplifier). Timing belts and gears are examples of this transmission type, and are illustrated + * below. \image html simple_transmission.png * *
* @@ -63,17 +63,20 @@ namespace transmission_interface * * * where: - * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively. - * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively. - * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates. - * - \f$ n \f$ is the transmission ratio, and can be computed as the ratio between the output and input pulley - * radii for the timing belt; or the ratio between output and input teeth for the gear system. - * The transmission ratio can take any real value \e except zero. In particular: - * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute - * value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. - * - Negative values represent a direction flip, ie. actuator and joint move in opposite directions. For example, - * in timing belts actuator and joint move in the same direction, while in single-stage gear systems actuator and - * joint move in opposite directions. + * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, + * respectively. + * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space + * variables, respectively. + * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position + * coordinates. + * - \f$ n \f$ is the transmission ratio, and can be computed as the ratio between the output and + * input pulley radii for the timing belt; or the ratio between output and input teeth for the gear + * system. The transmission ratio can take any real value \e except zero. In particular: + * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, + * while if its absolute value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. + * - Negative values represent a direction flip, ie. actuator and joint move in opposite + * directions. For example, in timing belts actuator and joint move in the same direction, while in + * single-stage gear systems actuator and joint move in opposite directions. * * \ingroup transmission_types */ @@ -102,14 +105,16 @@ class SimpleTransmission : public Transmission /// Transform variables from actuator to joint space. /** * This method operates on the handles provided when configuring the transmission. - * To call this method it is not required that all supported interface types are provided, e.g. one can supply only velocity handles + * To call this method it is not required that all supported interface types are provided, e.g. + * one can supply only velocity handles */ void actuator_to_joint() override; /// Transform variables from joint to actuator space. /** * This method operates on the handles provided when configuring the transmission. - * To call this method it is not required that all supported interface types are provided, e.g. one can supply only velocity handles + * To call this method it is not required that all supported interface types are provided, e.g. + * one can supply only velocity handles */ void joint_to_actuator() override; diff --git a/transmission_interface/include/transmission_interface/transmission.hpp b/transmission_interface/include/transmission_interface/transmission.hpp index 8ca0196c4c..697e1a4eb7 100644 --- a/transmission_interface/include/transmission_interface/transmission.hpp +++ b/transmission_interface/include/transmission_interface/transmission.hpp @@ -26,21 +26,23 @@ namespace transmission_interface { /// Abstract base class for representing mechanical transmissions. /** - * Mechanical transmissions transform effort/flow variables such that their product (power) remains constant. - * Effort variables for linear and rotational domains are \e force and \e torque; while the flow variables are - * respectively linear velocity and angular velocity. + * Mechanical transmissions transform effort/flow variables such that their product (power) remains + * constant. Effort variables for linear and rotational domains are \e force and \e 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. + * 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. * * \par Credit - * This interface was inspired by similar existing implementations by PAL Robotics, S.L. and Willow Garage Inc. + * This interface was inspired by similar existing implementations by PAL Robotics, S.L. and Willow + * Garage Inc. * - * \note Implementations of this interface must take care of realtime-safety if the code is to be run in realtime - * contexts, as is often the case in robot control. + * \note Implementations of this interface must take care of realtime-safety if the code is to be + * run in realtime contexts, as is often the case in robot control. */ class Transmission { @@ -55,20 +57,20 @@ class Transmission /** * \param[in] act_data Actuator-space variables. * \param[out] jnt_data Joint-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the + * number of transmission actuators and joints. Data vectors not used in this map can remain + * empty. */ virtual void actuator_to_joint() = 0; /// Transform \e effort variables from joint to actuator space. /** - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. - */ + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the + * number of transmission actuators and joints. Data vectors not used in this map can remain + * empty. + */ virtual void joint_to_actuator() = 0; /** \return Number of actuators managed by transmission, diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 8297097348..99d864bf6a 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.12.1 + 3.13.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl