From c122455ddc9db6b83aa7c69c797a7b5c6b3c00ef Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 24 Aug 2023 22:20:29 +0200 Subject: [PATCH 01/22] [Docs] Fix information about activation and deactivation of chainable controllers (#1104) --- .../doc/controller_chaining.rst | 44 +++---------------- 1 file changed, 5 insertions(+), 39 deletions(-) diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst index 437cb70509..d79e730d3f 100644 --- a/controller_manager/doc/controller_chaining.rst +++ b/controller_manager/doc/controller_chaining.rst @@ -63,48 +63,14 @@ After configuring a chainable controller, controller manager calls ``input_inter This is the same process as done by ``ResourceManager`` and hardware interfaces. Controller manager maintains "claimed" status of interface in a vector (the same as done in ``ResourceManager``). + Activation and Deactivation Chained Controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Controller Manager has an additional parameter that describes how controllers are chained. -In the first version, the parameter-structure would have some semantic meaning embedded into it, as follows: - - -.. code-block:: yaml - - controller_manager: - ros__parameters: - chained_controllers: - - - parallel_group_1: - - controller1_1 - - controller1_2 - - - parallel_group_2: - - controller2_1 - - - parallel_group_3: - - controller3_1 - - controller3_2 - - controller3_3 - - ... - - - parallel_group_N: - - controllerN_1 - - ... - - controllerN_M - - - -This structure is motivated by ``filter_chain`` structure from `ros/filters repository `__, see `this file for implementation `__. - -This structure is stored internally by controller manager into an ordered map (``std::map>``) with group name as key. -When a controller should be deactivated, the controller manager deactivates all the controllers in the preceding groups first. -All other controllers from the group stay active, as well as all controllers in the following groups. -NOTE: In the future this could be done more intelligently, i.e., deactivate only controllers in the preceding groups that actually precede the controller that should be deactivated. - -On the other hand, the controller should be manually activated in the reverse order, i.e., from the those closer to the hardware toward those preceding them. +Chained controllers must be activated and deactivated together or in the proper order. +This means you must first activate all following controllers to have the preceding one activated. +For the deactivation there is the inverse rule - all preceding controllers have to be deactivated before the following controller is deactivated. +One can also think of it as an actual chain, you can not add a chain link or break the chain in the middle. Debugging outputs From 1fa9afc21569348a794112d460b90fba3ccb3d31 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 11 Sep 2023 14:14:17 +0200 Subject: [PATCH 02/22] Bump actions/upload-artifact from 3.1.2 to 3.1.3 (#1111) --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index b339b45e66..5e439b3899 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -49,7 +49,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.2 + - uses: actions/upload-artifact@v3.1.3 with: name: colcon-logs-ubuntu-22.04-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 6cd59ce5f9..d4ee6e85a4 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -49,7 +49,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_control.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v3.1.2 + - uses: actions/upload-artifact@v3.1.3 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log From 3c5ad102e0aba34a8714b56f798b1161d32172aa Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 11 Sep 2023 14:15:18 +0200 Subject: [PATCH 03/22] Bump actions/checkout from 3 to 4 (#1112) --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-format.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/docker.yml | 4 ++-- .github/workflows/humble-abi-compatibility.yml | 2 +- .github/workflows/humble-rhel-binary-build.yml | 2 +- .github/workflows/iron-abi-compatibility.yml | 2 +- .github/workflows/iron-rhel-binary-build.yml | 2 +- .github/workflows/prerelease-check.yml | 2 +- .github/workflows/reusable-industrial-ci-with-cache.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- .github/workflows/reviewer_lottery.yml | 2 +- .github/workflows/rolling-rhel-binary-build.yml | 2 +- 13 files changed, 16 insertions(+), 16 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 5e439b3899..bb0c1a5434 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -19,7 +19,7 @@ jobs: - uses: ros-tooling/setup-ros@0.7.0 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-tooling/action-ros-ci@0.3.3 with: target-ros2-distro: ${{ env.ROS_DISTRO }} diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 9b91657851..d1ea84222c 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: name: Format runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: actions/setup-python@v4.7.0 with: python-version: '3.10' diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 37c371ce83..3efd557adc 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -13,7 +13,7 @@ jobs: env: AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@0.7.0 - uses: ros-tooling/action-ros-lint@v0.1 with: @@ -37,7 +37,7 @@ jobs: matrix: linter: [cpplint] steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@0.7.0 - uses: ros-tooling/action-ros-lint@v0.1 with: diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 9f3858d59f..3681701dab 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -22,7 +22,7 @@ jobs: uses: docker/setup-buildx-action@v2 - name: Checkout repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Log in to the Container registry uses: docker/login-action@v2 @@ -62,7 +62,7 @@ jobs: uses: docker/setup-buildx-action@v2 - name: Checkout repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Log in to the Container registry uses: docker/login-action@v2 diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 3f38d5b6ce..708ea5c1f4 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -11,7 +11,7 @@ jobs: abi_check: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: humble diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 589eda663f..0267849692 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -21,7 +21,7 @@ jobs: ROS_DISTRO: humble container: ghcr.io/ros-controls/ros:humble-rhel steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: src/ros2_control - run: | diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml index f92166b0c1..5bce340193 100644 --- a/.github/workflows/iron-abi-compatibility.yml +++ b/.github/workflows/iron-abi-compatibility.yml @@ -11,7 +11,7 @@ jobs: abi_check: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: iron diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index afcdf332d4..9db28cf987 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -21,7 +21,7 @@ jobs: ROS_DISTRO: iron container: ghcr.io/ros-controls/ros:iron-rhel steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: src/ros2_control - run: | diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index eefb15ae87..182df6e22b 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -26,7 +26,7 @@ jobs: pre_release: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: ref: ${{ github.event.inputs.branch }} - name: industrial_ci diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index bca08ce5d2..acefeebfac 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -58,10 +58,10 @@ jobs: steps: - name: Checkout ${{ inputs.ref }} when build is not scheduled if: ${{ github.event_name != 'schedule' }} - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Checkout ${{ inputs.ref }} on scheduled build if: ${{ github.event_name == 'schedule' }} - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: ref: ${{ inputs.ref_for_scheduled_build }} - name: cache target_ws diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index d4ee6e85a4..d7484dee0a 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -29,7 +29,7 @@ jobs: - uses: ros-tooling/setup-ros@0.7.0 with: required-ros-distributions: ${{ inputs.ros_distro }} - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: ref: ${{ inputs.ref }} - uses: ros-tooling/action-ros-ci@0.3.3 diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 772809825f..2edbc9b59e 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -7,7 +7,7 @@ jobs: test: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: uesteibar/reviewer-lottery@v3 with: repo-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index a4faa537af..0768fbfd68 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -17,7 +17,7 @@ jobs: ROS_DISTRO: rolling container: ghcr.io/ros-controls/ros:rolling-rhel steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: src/ros2_control - run: | From af1052537558450f57f0e647448be21399d283af Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 16 Sep 2023 21:10:56 +0200 Subject: [PATCH 04/22] Fix multiple calls to export reference interfaces (#1108) --- controller_manager/src/controller_manager.cpp | 14 ++++++++++---- .../test/test_controller_manager_srvs.cpp | 5 ++++- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 062d813f0d..c13eb74a80 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1639,11 +1639,17 @@ void ControllerManager::list_controllers_srv_cb( } } // check reference interfaces only if controller is inactive or active - auto references = controllers[i].c->export_reference_interfaces(); - controller_state.reference_interfaces.reserve(references.size()); - for (const auto & reference : references) + if (controllers[i].c->is_chainable()) { - controller_state.reference_interfaces.push_back(reference.get_interface_name()); + auto references = + resource_manager_->get_controller_reference_interface_names(controllers[i].info.name); + controller_state.reference_interfaces.reserve(references.size()); + for (const auto & reference : references) + { + const std::string prefix_name = controllers[i].c->get_node()->get_name(); + const std::string interface_name = reference.substr(prefix_name.size() + 1); + controller_state.reference_interfaces.push_back(interface_name); + } } } response->controller.push_back(controller_state); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 583480d494..7873adaacf 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -269,7 +269,9 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) ASSERT_EQ(result->controller[0].is_chainable, true); ASSERT_EQ(result->controller[0].is_chained, false); ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u); - ; + ASSERT_EQ("joint1/position", result->controller[0].reference_interfaces[0]); + ASSERT_EQ("joint1/velocity", result->controller[0].reference_interfaces[1]); + ASSERT_EQ(result->controller[0].chain_connections.size(), 0u); // check test controller ASSERT_EQ(result->controller[1].state, "inactive"); @@ -300,6 +302,7 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) test_chainable_controller::TEST_CONTROLLER_NAME, result->controller[0].chain_connections[0].name); ASSERT_EQ(2u, result->controller[0].chain_connections[0].reference_interfaces.size()); + ASSERT_EQ("test_chainable_controller_name", result->controller[0].chain_connections[0].name); ASSERT_EQ("joint1/position", result->controller[0].chain_connections[0].reference_interfaces[0]); ASSERT_EQ("joint1/velocity", result->controller[0].chain_connections[0].reference_interfaces[1]); } From b7850f3c75666d0b926fbd5dd85aae4dcd36240a Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 18 Sep 2023 08:49:16 +0200 Subject: [PATCH 05/22] Enable services for setting the log-level in controller per default (#1102) --- .../include/controller_interface/controller_interface_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 05ee0c8830..52bc970500 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -115,7 +115,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual return_type init( const std::string & controller_name, const std::string & namespace_ = "", - const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()); + const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true)); /// Custom configure method to read additional parameters for controller-nodes /* From 37a11bf081c05406888f5d5ee8acfa94f9c073b1 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 18 Sep 2023 18:31:37 +0100 Subject: [PATCH 06/22] Bump docker/setup-buildx-action from 2 to 3 (#1115) --- .github/workflows/docker.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 3681701dab..ad26b9ef00 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -19,7 +19,7 @@ jobs: steps: - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 + uses: docker/setup-buildx-action@v3 - name: Checkout repository uses: actions/checkout@v4 @@ -59,7 +59,7 @@ jobs: steps: - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 + uses: docker/setup-buildx-action@v3 - name: Checkout repository uses: actions/checkout@v4 From 14e9e4f696f14209febf28316de8cbdd68659605 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 18 Sep 2023 18:32:13 +0100 Subject: [PATCH 07/22] Bump docker/login-action from 2 to 3 (#1116) --- .github/workflows/docker.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index ad26b9ef00..f22d114cf2 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -25,7 +25,7 @@ jobs: uses: actions/checkout@v4 - name: Log in to the Container registry - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: registry: ${{ env.REGISTRY }} username: ${{ github.actor }} @@ -65,7 +65,7 @@ jobs: uses: actions/checkout@v4 - name: Log in to the Container registry - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: registry: ${{ env.REGISTRY }} username: ${{ github.actor }} From 49ea423f4954cab7e0895288a4f6b67479466449 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 19 Sep 2023 10:26:53 +0100 Subject: [PATCH 08/22] Bump docker/build-push-action from 4 to 5 (#1117) --- .github/workflows/docker.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index f22d114cf2..759276deb0 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -42,7 +42,7 @@ jobs: type=semver,pattern={{major}}.{{minor}} - name: Build and push Docker image - uses: docker/build-push-action@v4 + uses: docker/build-push-action@v5 with: context: . push: true @@ -82,7 +82,7 @@ jobs: type=semver,pattern={{major}}.{{minor}} - name: Build and push Docker image - uses: docker/build-push-action@v4 + uses: docker/build-push-action@v5 with: context: . push: true From a6a86d44c202652f8bda70b8b0ec385a96b14e81 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 19 Sep 2023 10:27:17 +0100 Subject: [PATCH 09/22] Bump docker/metadata-action from 4 to 5 (#1118) --- .github/workflows/docker.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 759276deb0..f5015d8a90 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -33,7 +33,7 @@ jobs: - name: Docker meta id: meta - uses: docker/metadata-action@v4 + uses: docker/metadata-action@v5 with: images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}_release tags: | @@ -73,7 +73,7 @@ jobs: - name: Docker meta id: meta - uses: docker/metadata-action@v4 + uses: docker/metadata-action@v5 with: images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}_source tags: | From d77f6f101d3e0c9063d467628e6744c1a644c41b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 21 Sep 2023 07:03:09 +0100 Subject: [PATCH 10/22] Trigger coverage build on the master branch (#1119) --- .github/workflows/ci-coverage-build.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index bb0c1a5434..1d506f3438 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -3,6 +3,9 @@ on: workflow_dispatch: branches: - master + push: + branches: + - master pull_request: branches: - master From 8141212696037eb32bf144f66e2d85e1357b18d1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 23 Sep 2023 10:57:07 +0200 Subject: [PATCH 11/22] Add GPIO tag description to docs (#1109) --- .../doc/hardware_components_userdoc.rst | 32 ++-- .../doc/hardware_interface_types_userdoc.rst | 150 ++++++++++++++++++ ...rst => writing_new_hardware_component.rst} | 8 +- 3 files changed, 175 insertions(+), 15 deletions(-) create mode 100644 hardware_interface/doc/hardware_interface_types_userdoc.rst rename hardware_interface/doc/{writing_new_hardware_interface.rst => writing_new_hardware_component.rst} (98%) diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index cd93f472a5..1a8c5e611b 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -14,20 +14,21 @@ Guidelines and Best Practices .. toctree:: :titlesonly: - Writing a Hardware Interface + Hardware Interface Types + Writing a Hardware Component Handling of errors that happen during read() and write() calls ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``write()`` methods of a hardware interface, ``on_error(previous_state)`` method will be called to handle any error that happened. +If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``write()`` methods of a hardware_interface class, ``on_error(previous_state)`` method will be called to handle any error that happened. Error handling follows the `node lifecycle `_. If successful ``CallbackReturn::SUCCESS`` is returned and hardware is again in ``UNCONFIGURED`` state, if any ``ERROR`` or ``FAILURE`` happens the hardware ends in ``FINALIZED`` state and can not be recovered. The only option is to reload the complete plugin, but there is currently no service for this in the Controller Manager. -Migration from Foxy to Galactic -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Migration from Foxy to newer versions +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle. The following list shows a set of quick changes to port existing hardware components to Galactic: @@ -36,20 +37,29 @@ The following list shows a set of quick changes to port existing hardware compon 2. If using BaseInterface as base class then you should remove it. Specifically, change: -hardware_interface::BaseInterface to hardware_interface::[Actuator|Sensor|System]Interface + .. code-block:: c++ + + hardware_interface::BaseInterface + + to + + .. code-block:: c++ + + hardware_interface::[Actuator|Sensor|System]Interface 3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp`` 4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary -5. replace first three lines in ``on_init`` to: +5. replace first three lines in ``on_init`` to + + .. code-block:: c++ -.. code-block:: c++ + if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } - if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) - { - return CallbackReturn::ERROR; - } 6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;`` diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst new file mode 100644 index 0000000000..54b2003568 --- /dev/null +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -0,0 +1,150 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/hardware_interface_types_userdoc.rst + +.. _hardware_interface_types_userdoc: + +``ros2_control`` hardware interface types +--------------------------------------------------------- + +The ``ros2_control`` framework provides a set of hardware interface types that can be used to implement +a hardware component for a specific robot or device. +The following sections describe the different hardware interface types and their usage. + +Joints +***************************** +````-tag groups the interfaces associated with the joints of physical robots and actuators. +They have command and state interfaces to set the goal values for hardware and read its current state. + +State interfaces of joints can be published as a ROS topic by means of the :ref:`joint_state_broadcaster ` + +Sensors +***************************** +````-tag groups multiple state interfaces describing, e.g., internal states of hardware. + +Depending on the type of sensor, there exist a couple of specific semantic components with broadcasters shipped with ros2_controllers, e.g. + +- :ref:`Imu Sensor Broadcaster ` +- :ref:`Force Torque Sensor Broadcaster ` + +GPIOs +***************************** +The ````-tag is used for describing input and output ports of a robotic device that cannot be associated with any joint or sensor. +Parsing of ````-tag is similar to this of a ````-tag having command and state interfaces. +The tag must have at least one ````- or ````-tag as a child. + +The keyword "gpio" is chosen for its generality. +Although strictly used for digital signals, it describes any electrical analog, digital signal, or physical value. + +The ```` tag can be used as a child of all three types of hardware components, i.e., system, sensor, or actuator. + +Because ports implemented as ````-tag are typically very application-specific, there exists no generic publisher +within the ros2_control framework. A custom gpio-controller has to be implemented for each application. As an example, see :ref:`the GPIO controller example ` as part of the demo repository. + +Examples +***************************** +The following examples show how to use the different hardware interface types in a ``ros2_control`` URDF. +They can be combined together within the different hardware component types (system, actuator, sensor) (:ref:`see detailed documentation `) as follows + +1. Robot with multiple GPIO interfaces + + - RRBot System + - Digital: 4 inputs and 2 outputs + - Analog: 2 inputs and 1 output + - Vacuum valve at the flange (on/off) + + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + + + + + + + + + + + + + +2. Gripper with electrical and suction grasping possibilities + + - Multimodal gripper + - 1-DoF parallel gripper + - suction on/off + + .. code:: xml + + + + ros2_control_demo_hardware/MultimodalGripper + + + + 0 + 100 + + + + + + + + + +3. Force-Torque-Sensor with temperature feedback and adjustable calibration + + - 2D FTS + - Temperature feedback in °C + - Choice between 3 calibration matrices, i.e., calibration ranges + + .. code:: xml + + + + ros2_control_demo_hardware/ForceTorqueSensor2DHardware + 0.43 + + + + + kuka_tcp + 100 + 100 + + + + + + + + + diff --git a/hardware_interface/doc/writing_new_hardware_interface.rst b/hardware_interface/doc/writing_new_hardware_component.rst similarity index 98% rename from hardware_interface/doc/writing_new_hardware_interface.rst rename to hardware_interface/doc/writing_new_hardware_component.rst index 1ff4dc4420..698f6cf6e2 100644 --- a/hardware_interface/doc/writing_new_hardware_interface.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -1,9 +1,9 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/writing_new_hardware_interface.rst +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/writing_new_hardware_component.rst -.. _writing_new_hardware_interface: +.. _writing_new_hardware_component: -Writing a new hardware interface -================================= +Writing a Hardware Component +============================ In ros2_control hardware system components are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new hardware interface. From ef5661940d9d3b1ea2e2dadf6809fa6fdc524ec3 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 2 Oct 2023 12:37:49 +0100 Subject: [PATCH 12/22] Bump actions/setup-python from 4.7.0 to 4.7.1 (#1124) --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index d1ea84222c..2e1400f718 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4.7.0 + - uses: actions/setup-python@v4.7.1 with: python-version: '3.10' - name: Install system hooks From 28711db71ab4867e26376b1cbb5aea0d27f973b9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Oct 2023 19:50:04 +0200 Subject: [PATCH 13/22] Proper controller update rate (#1105) --- .../controller_manager/controller_manager.hpp | 1 + .../controller_manager/controller_spec.hpp | 2 + controller_manager/src/controller_manager.cpp | 42 +++++++++++-------- .../test/test_controller_manager.cpp | 19 +++++---- 4 files changed, 39 insertions(+), 25 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 2850b2be80..3ef8882a5b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -122,6 +122,7 @@ class ControllerManager : public rclcpp::Node controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; + controller_spec.next_update_cycle_time = std::make_shared(0); return add_controller_impl(controller_spec); } diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 63823f8d33..6f7483f3ec 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -20,6 +20,7 @@ #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ #include +#include #include #include #include "controller_interface/controller_interface.hpp" @@ -37,6 +38,7 @@ struct ControllerSpec { hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; + std::shared_ptr next_update_cycle_time; }; } // namespace controller_manager diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c13eb74a80..6bb37a13b0 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -565,6 +565,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; + controller_spec.next_update_cycle_time = std::make_shared(0); return add_controller_impl(controller_spec); } @@ -740,16 +741,13 @@ controller_interface::return_type ControllerManager::configure_controller( } else if (controller_update_rate != 0 && cm_update_rate % controller_update_rate != 0) { - // NOTE: The following computation is done to compute the approx controller update that can be - // achieved w.r.t to the CM's update rate. This is done this way to take into account the - // unsigned integer division. - const auto act_ctrl_update_rate = cm_update_rate / (cm_update_rate / controller_update_rate); RCLCPP_WARN( get_logger(), - "The controller : %s update rate : %d Hz is not a perfect divisor of the controller " - "manager's update rate : %d Hz!. The controller will be updated with nearest divisor's " - "update rate which is : %d Hz.", - controller_name.c_str(), controller_update_rate, cm_update_rate, act_ctrl_update_rate); + "The controller : %s update cycles won't be triggered at a constant period : %f sec, as the " + "controller's update rate : %d Hz is not a perfect divisor of the controller manager's " + "update rate : %d Hz!.", + controller_name.c_str(), 1.0 / controller_update_rate, controller_update_rate, + cm_update_rate); } // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers @@ -1458,6 +1456,8 @@ void ControllerManager::activate_controllers( } auto controller = found_it->c; auto controller_name = found_it->info.name; + // reset the next update cycle time for newly activated controllers + *found_it->next_update_cycle_time = rclcpp::Time(0); bool assignment_successful = true; // assign command interfaces to the controller @@ -2030,19 +2030,22 @@ controller_interface::return_type ControllerManager::update( ++update_loop_counter_; update_loop_counter_ %= update_rate_; - for (auto loaded_controller : rt_controller_list) + for (const auto & loaded_controller : rt_controller_list) { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c)) { const auto controller_update_rate = loaded_controller.c->get_update_rate(); - const auto controller_update_factor = - (controller_update_rate == 0) || (controller_update_rate >= update_rate_) - ? 1u - : update_rate_ / controller_update_rate; + const bool run_controller_at_cm_rate = + (controller_update_rate == 0) || (controller_update_rate >= update_rate_); + const auto controller_period = + run_controller_at_cm_rate ? period + : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); + + bool controller_go = (time == rclcpp::Time(0)) || + (time.seconds() >= loaded_controller.next_update_cycle_time->seconds()); - bool controller_go = ((update_loop_counter_ % controller_update_factor) == 0); RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", update_loop_counter_, controller_go ? "True" : "False", @@ -2050,10 +2053,13 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - auto controller_ret = loaded_controller.c->update( - time, (controller_update_factor != 1u) - ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate) - : period); + auto controller_ret = loaded_controller.c->update(time, controller_period); + + if (*loaded_controller.next_update_cycle_time == rclcpp::Time(0)) + { + *loaded_controller.next_update_cycle_time = time; + } + *loaded_controller.next_update_cycle_time += controller_period; if (controller_ret != controller_interface::return_type::OK) { diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 6db4cfd1b2..b4d66f9039 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -423,23 +423,28 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); const auto cm_update_rate = cm_->get_update_rate(); const auto controller_update_rate = test_controller->get_update_rate(); - const auto controller_factor = (cm_update_rate / controller_update_rate); - const auto expected_controller_update_rate = - static_cast(std::round(cm_update_rate / static_cast(controller_factor))); const auto initial_counter = test_controller->internal_counter; - for (size_t update_counter = 1; update_counter <= 10 * cm_update_rate; ++update_counter) + rclcpp::Time time = rclcpp::Time(0); + for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time, rclcpp::Duration::from_seconds(0.01))); + time += rclcpp::Duration::from_seconds(0.01); if (update_counter % cm_update_rate == 0) { const auto no_of_secs_passed = update_counter / cm_update_rate; - EXPECT_EQ( + // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole + // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it + // is clearly tracking, so adding 1 here won't affect the final count. + // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update + // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 + // cycles it will have 369 instead of 370. + EXPECT_NEAR( test_controller->internal_counter - initial_counter, - (expected_controller_update_rate * no_of_secs_passed)); + (controller_update_rate * no_of_secs_passed), 1); } } } From f9499490a821b3c64f4edddf722a63f9b3c5b510 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 3 Oct 2023 20:02:55 +0200 Subject: [PATCH 14/22] [MockHardware] Added dynamic simulation functionality. (#1028) --- .../mock_components/generic_system.hpp | 17 +- .../src/mock_components/generic_system.cpp | 237 +++++++++++++++++- .../mock_components/test_generic_system.cpp | 234 ++++++++++++++++- 3 files changed, 475 insertions(+), 13 deletions(-) diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index c244ee1254..e9b38de65d 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -32,6 +32,10 @@ namespace mock_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +static constexpr size_t POSITION_INTERFACE_INDEX = 0; +static constexpr size_t VELOCITY_INTERFACE_INDEX = 1; +static constexpr size_t ACCELERATION_INTERFACE_INDEX = 2; + class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::SystemInterface { public: @@ -41,6 +45,14 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector export_command_interfaces() override; + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + + return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -60,8 +72,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - const size_t POSITION_INTERFACE_INDEX = 0; - struct MimicJoint { std::size_t joint_index; @@ -115,6 +125,9 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::string custom_interface_with_following_offset_; size_t index_custom_interface_with_following_offset_; + bool calculate_dynamics_; + std::vector joint_control_mode_; + bool command_propagation_disabled_; }; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 13a5a8b679..3ae3568a9e 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -131,6 +131,17 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i command_propagation_disabled_ = false; } + // check if there is parameter that enables dynamic calculation + it = info_.hardware_parameters.find("calculate_dynamics"); + if (it != info.hardware_parameters.end()) + { + calculate_dynamics_ = hardware_interface::parse_bool(it->second); + } + else + { + calculate_dynamics_ = false; + } + // process parameters about state following position_state_following_offset_ = 0.0; custom_interface_with_following_offset_ = ""; @@ -312,7 +323,7 @@ std::vector GenericSystem::export_command_ std::vector command_interfaces; // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) + for (size_t i = 0; i < info_.joints.size(); ++i) { const auto & joint = info_.joints[i]; for (const auto & interface : joint.command_interfaces) @@ -333,6 +344,8 @@ std::vector GenericSystem::export_command_ } } } + // Set position control mode per default + joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) @@ -369,7 +382,135 @@ std::vector GenericSystem::export_command_ return command_interfaces; } -return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +return_type GenericSystem::prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & /*stop_interfaces*/) +{ + hardware_interface::return_type ret_val = hardware_interface::return_type::OK; + + const size_t FOUND_ONCE_FLAG = 1000000; + + std::vector joint_found_in_x_requests_; + joint_found_in_x_requests_.resize(info_.joints.size(), 0); + + for (const auto & key : start_interfaces) + { + // check if interface is joint + auto joint_it_found = std::find_if( + info_.joints.begin(), info_.joints.end(), + [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); + + if (joint_it_found != info_.joints.end()) + { + const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + if (joint_found_in_x_requests_[joint_index] == 0) + { + joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; + } + + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + { + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + if (!calculate_dynamics_) + { + RCUTILS_LOG_WARN_NAMED( + "mock_generic_system", + "Requested velocity mode for joint '%s' without dynamics calculation enabled - this " + "might lead to wrong feedback and unexpected behavior.", + info_.joints[joint_index].name.c_str()); + } + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + { + if (!calculate_dynamics_) + { + RCUTILS_LOG_WARN_NAMED( + "mock_generic_system", + "Requested acceleration mode for joint '%s' without dynamics calculation enabled - " + "this might lead to wrong feedback and unexpected behavior.", + info_.joints[joint_index].name.c_str()); + } + joint_found_in_x_requests_[joint_index] += 1; + } + } + else + { + RCUTILS_LOG_DEBUG_NAMED( + "mock_generic_system", "Got interface '%s' that is not joint - nothing to do!", + key.c_str()); + } + } + + for (size_t i = 0; i < info_.joints.size(); ++i) + { + // There has to always be at least one control mode from the above three set + if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) + { + RCUTILS_LOG_ERROR_NAMED( + "mock_generic_system", "Joint '%s' has to have '%s', '%s', or '%s' interface!", + info_.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); + ret_val = hardware_interface::return_type::ERROR; + } + + // Currently we don't support multiple interface request + if (joint_found_in_x_requests_[i] > (FOUND_ONCE_FLAG + 1)) + { + RCUTILS_LOG_ERROR_NAMED( + "mock_generic_system", + "Got multiple (%zu) starting interfaces for joint '%s' - this is not " + "supported!", + joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); + ret_val = hardware_interface::return_type::ERROR; + } + } + + return ret_val; +} + +return_type GenericSystem::perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & /*stop_interfaces*/) +{ + if (!calculate_dynamics_) + { + return hardware_interface::return_type::OK; + } + + for (const auto & key : start_interfaces) + { + // check if interface is joint + auto joint_it_found = std::find_if( + info_.joints.begin(), info_.joints.end(), + [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); + + if (joint_it_found != info_.joints.end()) + { + const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + { + joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + { + joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; + } + } + } + + return hardware_interface::return_type::OK; +} + +return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { if (command_propagation_disabled_) { @@ -392,19 +533,97 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } }; - // apply offset to positions only for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if (calculate_dynamics_) { - joint_states_[POSITION_INTERFACE_INDEX][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + switch (joint_control_mode_[j]) + { + case ACCELERATION_INTERFACE_INDEX: + { + // currently we do backward integration + joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only + joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + joint_states_[VELOCITY_INTERFACE_INDEX][j] += + joint_states_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds(); + + if (!std::isnan(joint_commands_[ACCELERATION_INTERFACE_INDEX][j])) + { + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + joint_commands_[ACCELERATION_INTERFACE_INDEX][j]; + } + break; + } + case VELOCITY_INTERFACE_INDEX: + { + // currently we do backward integration + joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only + joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + if (!std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j])) + { + const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[VELOCITY_INTERFACE_INDEX][j] = + joint_commands_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + } + break; + } + case POSITION_INTERFACE_INDEX: + { + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + { + const double old_position = joint_states_[POSITION_INTERFACE_INDEX][j]; + const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + joint_states_[VELOCITY_INTERFACE_INDEX][j] = + (joint_states_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds(); + + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + } + break; + } + } + } + else + { + for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + { + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + { + joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + } + } } } - // do loopback on all other interfaces - starts from 1 because 0 index is position interface - mirror_command_to_state(joint_states_, joint_commands_, 1); + // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, + // velocity, and acceleration interface + if (calculate_dynamics_) + { + mirror_command_to_state(joint_states_, joint_commands_, 3); + } + else + { + mirror_command_to_state(joint_states_, joint_commands_, 1); + } for (const auto & mimic_joint : mimic_joints_) { diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 8e78ade7fb..cd344b600e 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -33,7 +33,8 @@ namespace { const auto TIME = rclcpp::Time(0); -const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for easier math +const auto COMPARE_DELTA = 0.0001; } // namespace class TestGenericSystem : public ::testing::Test @@ -484,6 +485,39 @@ class TestGenericSystem : public ::testing::Test )"; + hardware_system_2dof_standard_interfaces_with_different_control_modes_ = + R"( + + + mock_components/GenericSystem + true + + + + + + 3.45 + + + + + + + + + 2.78 + + + + + + + + + + +)"; + disabled_commands_ = R"( @@ -521,6 +555,7 @@ class TestGenericSystem : public ::testing::Test 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_; + std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_; std::string disabled_commands_; }; @@ -1624,6 +1659,202 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) ASSERT_EQ(1, state.get_value()); } +TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) +{ + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_standard_interfaces_with_different_control_modes_ + + ros2_control_test_assets::urdf_tail; + + TestableResourceManager rm(urdf); + // Activate components to get all interfaces available + activate_components(rm); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(7u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint1/acceleration")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/acceleration")); + EXPECT_TRUE(rm.state_interface_exists("flange_vacuum/vacuum")); + + ASSERT_EQ(5u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/acceleration")); + EXPECT_TRUE(rm.command_interface_exists("flange_vacuum/vacuum")); + + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j1a_s = rm.claim_state_interface("joint1/acceleration"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface("joint2/velocity"); + hardware_interface::LoanedStateInterface j2a_s = rm.claim_state_interface("joint2/acceleration"); + hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j2a_c = + rm.claim_command_interface("joint2/acceleration"); + + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_TRUE(std::isnan(j2a_c.get_value())); + + // Test error management in prepare mode switch + ASSERT_EQ( // joint2 has non 'position', 'velocity', or 'acceleration' interface + rm.prepare_command_mode_switch({"joint1/position", "joint2/effort"}, {}), false); + ASSERT_EQ( // joint1 has two interfaces + rm.prepare_command_mode_switch({"joint1/position", "joint1/acceleration"}, {}), false); + + // switch controller mode as controller manager is doing - gpio itf 'vacuum' will be ignored + ASSERT_EQ( + rm.prepare_command_mode_switch( + {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), + true); + ASSERT_EQ( + rm.perform_command_mode_switch( + {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), + true); + + // set some new values in commands + j1p_c.set_value(0.11); + j2a_c.set_value(3.5); + + // State values should not be changed + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // write() does not change values + rm.write(TIME, PERIOD); + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(-33.4, j1v_s.get_value()); + EXPECT_NEAR(-334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_NEAR(334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_NEAR(0.35, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // switch controller mode as controller manager is doing + ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + ASSERT_EQ(rm.perform_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + + // set some new values in commands + j1v_c.set_value(0.5); + j2v_c.set_value(2.0); + + // State values should not be changed + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // write() does not change values + rm.write(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics (both velocity mode) + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(5.0, j1a_s.get_value()); + EXPECT_EQ(2.885, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_NEAR(13.0, j2a_s.get_value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics (both velocity mode) + rm.read(TIME, PERIOD); + EXPECT_EQ(0.16, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(3.085, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); +} + TEST_F(TestGenericSystem, disabled_commands_flag_is_active) { auto urdf = @@ -1641,7 +1872,6 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ASSERT_EQ(2u, rm.command_interface_keys().size()); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); - // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); From dd27066e6d26043fc9b8f1dcd2a9e443368ea706 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 3 Oct 2023 19:10:19 +0100 Subject: [PATCH 15/22] Update changelogs --- controller_interface/CHANGELOG.rst | 5 +++++ controller_manager/CHANGELOG.rst | 7 +++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 6 ++++++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 10 files changed, 39 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index aaa2667a06..73a22824a1 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Enable services for setting the log-level in controller per default (`#1102 `_) +* Contributors: Dr. Denis + 3.18.0 (2023-08-17) ------------------- * add a broadcaster for range sensor (`#1091 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 2a9e348b8e..6c88ef899e 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Proper controller update rate (`#1105 `_) +* Fix multiple calls to export reference interfaces (`#1108 `_) +* [Docs] Fix information about activation and deactivation of chainable controllers (`#1104 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + 3.18.0 (2023-08-17) ------------------- * Controller sorting and proper execution in a chain (Fixes `#853 `_) (`#1063 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 8072652c55..5de8b89e56 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.18.0 (2023-08-17) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 0428cf4f8e..6fa8815c7f 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [MockHardware] Added dynamic simulation functionality. (`#1028 `_) +* Add GPIO tag description to docs (`#1109 `_) +* Contributors: Christoph Fröhlich, Dr. Denis + 3.18.0 (2023-08-17) ------------------- diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 46acee3f8d..944cd45273 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.18.0 (2023-08-17) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 272c2d3fe4..86f5d6ca64 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.18.0 (2023-08-17) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index d633c415d1..ae706706b6 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.18.0 (2023-08-17) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index a5a153faf1..37b6080f7e 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.18.0 (2023-08-17) ------------------- diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 9996ff5a85..631331cb77 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.18.0 (2023-08-17) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index c45b040c1b..ccf3c0aa17 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.18.0 (2023-08-17) ------------------- From 77229e01badba042c99cf6f8cc1303a81608deff Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 3 Oct 2023 19:10:39 +0100 Subject: [PATCH 16/22] 3.19.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 22 files changed, 32 insertions(+), 32 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 73a22824a1..128d74c5d0 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.0 (2023-10-03) +------------------- * Enable services for setting the log-level in controller per default (`#1102 `_) * Contributors: Dr. Denis diff --git a/controller_interface/package.xml b/controller_interface/package.xml index d960bdf28a..b75a9bc7cf 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.18.0 + 3.19.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 6c88ef899e..043234d111 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.0 (2023-10-03) +------------------- * Proper controller update rate (`#1105 `_) * Fix multiple calls to export reference interfaces (`#1108 `_) * [Docs] Fix information about activation and deactivation of chainable controllers (`#1104 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index adb6769d9c..0847e93902 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.18.0 + 3.19.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 5de8b89e56..f6bfb59238 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.0 (2023-10-03) +------------------- 3.18.0 (2023-08-17) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index f7b9eb5642..d2d0529b81 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.18.0 + 3.19.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 6fa8815c7f..0f36efc080 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.0 (2023-10-03) +------------------- * [MockHardware] Added dynamic simulation functionality. (`#1028 `_) * Add GPIO tag description to docs (`#1109 `_) * Contributors: Christoph Fröhlich, Dr. Denis diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index e3a2f4f71e..ae27ce82f9 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.18.0 + 3.19.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 944cd45273..1ac507d046 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.0 (2023-10-03) +------------------- 3.18.0 (2023-08-17) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index f1c0365cfd..80023c862d 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.18.0 + 3.19.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 86f5d6ca64..7c7eb0e1ce 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.0 (2023-10-03) +------------------- 3.18.0 (2023-08-17) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 15a379933a..4d60641637 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.18.0 + 3.19.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 ae706706b6..f4b8865deb 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.0 (2023-10-03) +------------------- 3.18.0 (2023-08-17) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index c77b828276..8c434037e0 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.18.0 + 3.19.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 37b6080f7e..e99d1863a5 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.0 (2023-10-03) +------------------- 3.18.0 (2023-08-17) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 92eba51b62..cfde81a3e0 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.18.0 + 3.19.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index aa2b19b830..65873c82f6 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.18.0", + version="3.19.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 631331cb77..cc156d712e 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.0 (2023-10-03) +------------------- 3.18.0 (2023-08-17) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 4888459d49..c172ae2199 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.18.0 + 3.19.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 318cf556d2..3f9347e316 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.18.0", + version="3.19.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 ccf3c0aa17..056761f272 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.0 (2023-10-03) +------------------- 3.18.0 (2023-08-17) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 9e2b2fded2..27a8deb943 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.18.0 + 3.19.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 From 52218f87fe058758f7799ff47d831b9dd1fcae81 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Oct 2023 12:57:10 +0200 Subject: [PATCH 17/22] Fix next controller update cycle time clock (#1127) --- controller_manager/src/controller_manager.cpp | 16 +++++--- .../test/controller_manager_test_common.hpp | 13 ++++--- .../test/test_controller_manager.cpp | 39 +++++++++---------- ...roller_manager_hardware_error_handling.cpp | 34 ++++++++-------- ...llers_chaining_with_controller_manager.cpp | 2 +- .../test/test_spawner_unspawner.cpp | 6 +-- 6 files changed, 58 insertions(+), 52 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6bb37a13b0..a48069acfb 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -565,7 +565,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.next_update_cycle_time = std::make_shared( + 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); return add_controller_impl(controller_spec); } @@ -1457,7 +1458,8 @@ void ControllerManager::activate_controllers( auto controller = found_it->c; auto controller_name = found_it->info.name; // reset the next update cycle time for newly activated controllers - *found_it->next_update_cycle_time = rclcpp::Time(0); + *found_it->next_update_cycle_time = + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); bool assignment_successful = true; // assign command interfaces to the controller @@ -2043,8 +2045,10 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); - bool controller_go = (time == rclcpp::Time(0)) || - (time.seconds() >= loaded_controller.next_update_cycle_time->seconds()); + bool controller_go = + (time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || + (time.seconds() >= loaded_controller.next_update_cycle_time->seconds()); RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", @@ -2055,7 +2059,9 @@ controller_interface::return_type ControllerManager::update( { auto controller_ret = loaded_controller.c->update(time, controller_period); - if (*loaded_controller.next_update_cycle_time == rclcpp::Time(0)) + if ( + *loaded_controller.next_update_cycle_time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { *loaded_controller.next_update_cycle_time = time; } diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 78c3fcb06b..7bf5cae628 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -41,7 +41,6 @@ namespace { -const auto TIME = rclcpp::Time(0); const auto PERIOD = rclcpp::Duration::from_seconds(0.01); const auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT; const auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; @@ -101,6 +100,7 @@ class ControllerManagerFixture : public ::testing::Test cm_->robot_description_callback(msg); } } + time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); } static void SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -119,7 +119,7 @@ class ControllerManagerFixture : public ::testing::Test { while (run_updater_) { - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } }); @@ -157,6 +157,7 @@ class ControllerManagerFixture : public ::testing::Test bool run_updater_; const std::string robot_description_; const bool pass_urdf_as_parameter_; + rclcpp::Time time_; }; class TestControllerManagerSrvs @@ -177,9 +178,9 @@ class TestControllerManagerSrvs std::chrono::milliseconds(10), [&]() { - cm_->read(TIME, PERIOD); - cm_->update(TIME, PERIOD); - cm_->write(TIME, PERIOD); + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); }); executor_->add_node(cm_); @@ -206,7 +207,7 @@ class TestControllerManagerSrvs while (service_executor.spin_until_future_complete(result, std::chrono::milliseconds(50)) != rclcpp::FutureReturnCode::SUCCESS) { - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); } } else diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index b4d66f9039..e317726585 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -96,7 +96,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -108,7 +108,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) cm_->configure_controller(TEST_CONTROLLER2_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; @@ -123,7 +123,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -132,7 +132,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter); // Start the real test controller, will take effect at the end of the update function @@ -147,7 +147,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -157,7 +157,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_GE(test_controller->internal_counter, 1u); size_t last_internal_counter = test_controller->internal_counter; @@ -173,7 +173,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter) << "Controller is stopped at the end of update, so it should have done one more update"; { @@ -208,7 +208,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -220,7 +220,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -237,7 +237,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -251,7 +251,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); } EXPECT_GE(test_controller->internal_counter, 1u); EXPECT_EQ(test_controller->get_update_rate(), 4u); @@ -286,7 +286,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -299,7 +299,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -316,7 +316,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is started at the end of update"; { @@ -332,7 +332,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -348,8 +348,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd << "switch_controller should be blocking until next update cycle"; EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))) + controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))) << "Controller is stopped at the end of update, so it should have done one more update"; { ControllerManagerRunner cm_runner(this); @@ -393,7 +392,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -411,7 +410,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -425,7 +424,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) const auto controller_update_rate = test_controller->get_update_rate(); const auto initial_counter = test_controller->internal_counter; - rclcpp::Time time = rclcpp::Time(0); + rclcpp::Time time = time_; for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { EXPECT_EQ( diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 1ec725f8c5..20af889c81 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -138,7 +138,7 @@ class TestControllerManagerWithTestableCM cm_->configure_controller(TEST_BROADCASTER_ALL_NAME); cm_->configure_controller(TEST_BROADCASTER_SENSOR_NAME); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(0u, test_controller_actuator->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller_system->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_broadcaster_all->internal_counter) << "Controller is not started"; @@ -226,7 +226,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er hardware_interface::lifecycle_state_names::ACTIVE); { - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_GE(test_controller_actuator->internal_counter, 1u) << "Controller is started at the end of update"; EXPECT_GE(test_controller_system->internal_counter, 1u) @@ -249,7 +249,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Execute first time without any errors { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; @@ -261,7 +261,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -278,7 +278,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -289,7 +289,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) << "Execute has read error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -333,7 +333,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute without errors to write value"; @@ -350,7 +350,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er auto previous_counter_higher = test_controller_system->internal_counter; auto new_counter = test_broadcaster_sensor->internal_counter + 1; - EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -361,7 +361,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute has read error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) @@ -418,7 +418,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e hardware_interface::lifecycle_state_names::ACTIVE); { - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_GE(test_controller_actuator->internal_counter, 1u) << "Controller is started at the end of update"; EXPECT_GE(test_controller_system->internal_counter, 1u) @@ -441,7 +441,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Execute first time without any errors { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; @@ -453,7 +453,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -470,7 +470,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -481,7 +481,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -525,7 +525,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute without errors to write value"; @@ -544,7 +544,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -555,7 +555,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute has write error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 16ba39d953..a34d70ada7 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -378,7 +378,7 @@ class TestControllerChainingWithControllerManager position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // check if all controllers are updated diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index d67ae8a7d1..4137386f72 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -39,9 +39,9 @@ class TestLoadController : public ControllerManagerFixtureread(TIME, PERIOD); - cm_->update(TIME, PERIOD); - cm_->write(TIME, PERIOD); + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); }); update_executor_ = From 724be2f49f435e238da4dfe4acafb87216ce8d75 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 4 Oct 2023 11:58:12 +0100 Subject: [PATCH 18/22] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 5 +++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 10 files changed, 32 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 128d74c5d0..48d92d905d 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.19.0 (2023-10-03) ------------------- * Enable services for setting the log-level in controller per default (`#1102 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 043234d111..4834d4fa3c 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix next controller update cycle time clock (`#1127 `_) +* Contributors: Sai Kishor Kothakota + 3.19.0 (2023-10-03) ------------------- * Proper controller update rate (`#1105 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index f6bfb59238..2777d19900 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.19.0 (2023-10-03) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 0f36efc080..6e424bbda6 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.19.0 (2023-10-03) ------------------- * [MockHardware] Added dynamic simulation functionality. (`#1028 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 1ac507d046..37cc837c9e 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.19.0 (2023-10-03) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 7c7eb0e1ce..9fafc8e687 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.19.0 (2023-10-03) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index f4b8865deb..d02ccde772 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.19.0 (2023-10-03) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index e99d1863a5..d22c1bff3d 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.19.0 (2023-10-03) ------------------- diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index cc156d712e..03fd4968da 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.19.0 (2023-10-03) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 056761f272..6910d52ee4 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.19.0 (2023-10-03) ------------------- From d333922528ae009b3f30d31fe7ffcfe54edd45bb Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 4 Oct 2023 11:58:31 +0100 Subject: [PATCH 19/22] 3.19.1 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 22 files changed, 32 insertions(+), 32 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 48d92d905d..76b09e75a3 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.1 (2023-10-04) +------------------- 3.19.0 (2023-10-03) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index b75a9bc7cf..b9c89c0bfb 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.19.0 + 3.19.1 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 4834d4fa3c..0da7427315 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.1 (2023-10-04) +------------------- * Fix next controller update cycle time clock (`#1127 `_) * Contributors: Sai Kishor Kothakota diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 0847e93902..556c1f222e 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.19.0 + 3.19.1 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 2777d19900..ae3049ef44 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.1 (2023-10-04) +------------------- 3.19.0 (2023-10-03) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index d2d0529b81..b384585b22 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.19.0 + 3.19.1 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 6e424bbda6..7c14eba8a6 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.1 (2023-10-04) +------------------- 3.19.0 (2023-10-03) ------------------- diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index ae27ce82f9..c53c8523ec 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.19.0 + 3.19.1 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 37cc837c9e..c994efe036 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.1 (2023-10-04) +------------------- 3.19.0 (2023-10-03) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 80023c862d..fc2cd26f9e 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.19.0 + 3.19.1 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 9fafc8e687..5032992acf 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.1 (2023-10-04) +------------------- 3.19.0 (2023-10-03) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 4d60641637..ab9112ad81 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.19.0 + 3.19.1 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 d02ccde772..d1305ad796 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.1 (2023-10-04) +------------------- 3.19.0 (2023-10-03) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 8c434037e0..ccf21c5295 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.19.0 + 3.19.1 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index d22c1bff3d..ba85f2a928 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.1 (2023-10-04) +------------------- 3.19.0 (2023-10-03) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index cfde81a3e0..d91f66d4ea 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.19.0 + 3.19.1 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 65873c82f6..fb5c06e6f4 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.19.0", + version="3.19.1", 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 03fd4968da..5fd421b0cb 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.1 (2023-10-04) +------------------- 3.19.0 (2023-10-03) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index c172ae2199..3f56fd18b6 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.19.0 + 3.19.1 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 3f9347e316..001968714a 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.19.0", + version="3.19.1", 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 6910d52ee4..f507b8921f 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.19.1 (2023-10-04) +------------------- 3.19.0 (2023-10-03) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 27a8deb943..4d68170366 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.19.0 + 3.19.1 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 From e07736011bf28895596cf10dcb7cbcdb40d86daa Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sat, 7 Oct 2023 20:30:39 +0200 Subject: [PATCH 20/22] Export of the get_cm_node_options() from robostack (#1129) --- .../include/controller_manager/controller_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 3ef8882a5b..cf1e959e1f 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -58,7 +58,7 @@ namespace controller_manager { using ControllersListIterator = std::vector::const_iterator; -rclcpp::NodeOptions get_cm_node_options(); +CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options(); class ControllerManager : public rclcpp::Node { From 19c8ff8651433727e05154f13a0531dd705b2278 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 10 Oct 2023 14:38:56 +0200 Subject: [PATCH 21/22] Fix doc of load_controller (#1132) --- ros2controlcli/doc/userdoc.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 5880c054cb..5eada2acb6 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -148,7 +148,7 @@ load_controller .. code-block:: console $ ros2 control load_controller -h - usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [--set_state {configure,activate}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name + usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [--set-state {inactive,active}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name Load a controller in a controller manager @@ -159,7 +159,7 @@ load_controller -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) - --set_state {inactive,active} + --set-state {inactive,active} Set the state of the loaded controller -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node From bd6911d663abb687dc956bf19a104d03648f8985 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Mon, 16 Oct 2023 18:25:28 +0200 Subject: [PATCH 22/22] [ResourceManager] deactivate hardware from read/write return value (#884) * Deactivate hardware from read/write return value * Added tests to DEACTIVATE return value * Use constants for defining of special test-values for different HW behaviors --------- Co-authored-by: Dr. Denis --- ...roller_manager_hardware_error_handling.cpp | 17 +- hardware_interface/CMakeLists.txt | 4 +- .../hardware_interface_return_values.hpp | 1 + hardware_interface/src/resource_manager.cpp | 21 ++- .../test/test_components/test_actuator.cpp | 15 +- .../test/test_components/test_system.cpp | 15 +- .../test/test_resource_manager.cpp | 145 +++++++++++++++++- .../test_hardware_interface_constants.hpp | 28 ++++ 8 files changed, 225 insertions(+), 21 deletions(-) create mode 100644 ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 20af889c81..f12ab9e96b 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -22,6 +22,7 @@ #include "controller_manager_test_common.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" #include "test_controller/test_controller.hpp" using ::testing::_; @@ -162,10 +163,6 @@ class TestControllerManagerWithTestableCM {}, strictness); } - // values to set to hardware to simulate failure on read and write - static constexpr double READ_FAIL_VALUE = 28282828.0; - static constexpr double WRITE_FAIL_VALUE = 23232323.0; - static constexpr char TEST_CONTROLLER_ACTUATOR_NAME[] = "test_controller_actuator"; static constexpr char TEST_CONTROLLER_SYSTEM_NAME[] = "test_controller_system"; static constexpr char TEST_BROADCASTER_ALL_NAME[] = "test_broadcaster_all"; @@ -258,7 +255,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to // READ_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); @@ -327,8 +324,8 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME // by setting first command interface to READ_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; - test_controller_system->set_first_command_interface_value_to = READ_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; { auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; @@ -450,7 +447,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to // WRITE_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); @@ -519,8 +516,8 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME // by setting first command interface to WRITE_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; - test_controller_system->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; { auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 2d6c72ffae..e486302c4c 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -101,7 +101,9 @@ if(BUILD_TESTING) test/test_components/test_system.cpp) target_link_libraries(test_components hardware_interface) ament_target_dependencies(test_components - pluginlib) + pluginlib + ros2_control_test_assets + ) install(TARGETS test_components DESTINATION lib ) diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index 5c3ea22ca0..d39dee2c64 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -23,6 +23,7 @@ enum class return_type : std::uint8_t { OK = 0, ERROR = 1, + DEACTIVATE = 2, }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 73daddd23d..36ace2b78f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1259,12 +1259,24 @@ HardwareReadWriteStatus ResourceManager::read( { for (auto & component : components) { - if (component.read(time, period) != return_type::OK) + auto ret_val = component.read(time, period); + if (ret_val == return_type::ERROR) { read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); } + else if (ret_val == return_type::DEACTIVATE) + { + resource_storage_->deactivate_hardware(component); + } + // If desired: automatic re-activation. We could add a flag for this... + // else + // { + // using lifecycle_msgs::msg::State; + // rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + // set_component_state(component.get_name(), state); + // } } }; @@ -1287,12 +1299,17 @@ HardwareReadWriteStatus ResourceManager::write( { for (auto & component : components) { - if (component.write(time, period) != return_type::OK) + auto ret_val = component.write(time, period); + if (ret_val == return_type::ERROR) { read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); } + else if (ret_val == return_type::DEACTIVATE) + { + resource_storage_->deactivate_hardware(component); + } } }; diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 5f9c09e95e..7cf55b50d3 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -16,6 +16,7 @@ #include #include "hardware_interface/actuator_interface.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::ActuatorInterface; using hardware_interface::CommandInterface; @@ -76,12 +77,17 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_ == 28282828.0) + if (velocity_command_ == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_ = 0.0; return return_type::ERROR; } + // simulate deactivate on read + if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } // The next line is for the testing purposes. We need value to be changed to be sure that // the feedback from hardware to controllers in the chain is working as it should. // This makes value checks clearer and confirms there is no "state = command" line or some @@ -93,12 +99,17 @@ class TestActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_ == 23232323.0) + if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_ = 0.0; return return_type::ERROR; } + // simulate deactivate on write + if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } return return_type::OK; } diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 7477734cff..3326cb893b 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -18,6 +18,7 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::CommandInterface; using hardware_interface::return_type; @@ -81,24 +82,34 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_[0] == 28282828) + if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_[0] = 0.0; return return_type::ERROR; } + // simulate deactivate on read + if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } return return_type::OK; } return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_[0] == 23232323) + if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_[0] = 0.0; return return_type::ERROR; } + // simulate deactivate on write + if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } return return_type::OK; } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index aedf2b862d..c80cdbb5bf 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -28,6 +28,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; @@ -1538,6 +1539,122 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest } } + void check_read_or_write_deactivate( + FunctionT method_that_deactivates, FunctionT other_method, const double deactivate_value) + { + // define state to set components to + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + // deactivate for TEST_ACTUATOR_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value); + claimed_itfs[1].set_value(deactivate_value - 10.0); + { + // deactivate on error + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + + // reactivate + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // deactivate for TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value - 10.0); + claimed_itfs[1].set_value(deactivate_value); + { + // deactivate on flag + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + check_if_interface_available(true, true); + // re-activate + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value); + claimed_itfs[1].set_value(deactivate_value); + { + // deactivate on flag + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + check_if_interface_available(true, true); + // re-activate + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + } + public: std::shared_ptr rm; std::vector claimed_itfs; @@ -1546,8 +1663,6 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); // values to set to hardware to simulate failure on read and write - static constexpr double READ_FAIL_VALUE = 28282828.0; - static constexpr double WRITE_FAIL_VALUE = 23232323.0; }; TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read) @@ -1558,7 +1673,7 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read) // check read methods failures check_read_or_write_failure( std::bind(&TestableResourceManager::read, rm, _1, _2), - std::bind(&TestableResourceManager::write, rm, _1, _2), READ_FAIL_VALUE); + std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_FAIL_VALUE); } TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) @@ -1569,7 +1684,29 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) // check write methods failures check_read_or_write_failure( std::bind(&TestableResourceManager::write, rm, _1, _2), - std::bind(&TestableResourceManager::read, rm, _1, _2), WRITE_FAIL_VALUE); + std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_FAIL_VALUE); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_read) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check read methods failures + check_read_or_write_deactivate( + std::bind(&TestableResourceManager::read, rm, _1, _2), + std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_DEACTIVATE_VALUE); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_write) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check write methods failures + check_read_or_write_deactivate( + std::bind(&TestableResourceManager::write, rm, _1, _2), + std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_DEACTIVATE_VALUE); } TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp new file mode 100644 index 0000000000..eb2bbf24c7 --- /dev/null +++ b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp @@ -0,0 +1,28 @@ +// Copyright (c) 2023, FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Felix Exner + +#ifndef ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ +#define ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ +namespace test_constants +{ +/// Constants defining special values used inside tests to trigger things like deactivate or errors +/// on read/write. +constexpr double READ_FAIL_VALUE = 28282828.0; +constexpr double WRITE_FAIL_VALUE = 23232323.0; +constexpr double READ_DEACTIVATE_VALUE = 29292929.0; +constexpr double WRITE_DEACTIVATE_VALUE = 24242424.0; +} // namespace test_constants +#endif // ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_