diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index 50c707e12b..c6580eacd4 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -13,24 +13,21 @@ groups: - name: reviewers reviewers: 5 usernames: - - rosterloh - - progtologist + - aprotyas - arne48 + - bijoua29 + - christophfroehlich - DasRoteSkelett - - Serafadam - - harderthan - - jaron-l - - malapatiravi - - homalozoa + - duringhof - erickisos - - anfemosa - - jackcenter - - VX792 - - mhubii + - fmauch + - jaron-l - livanov93 - - aprotyas - - peterdavidfagan - - duringhof - - bijoua29 - - lm2292 - mcbed + - moriarty + - olivier-stasse + - peterdavidfagan + - progtologist + - saikishor + - VanshGehlot + - VX792 diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index a2d02ed8a0..c184009187 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,11 +16,11 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.3.4 + - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.2.6 + - uses: actions/checkout@v4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} @@ -44,12 +44,12 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.0 + - uses: codecov/codecov-action@v3.1.4 with: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.0 + - uses: actions/upload-artifact@v4 with: name: colcon-logs-ubuntu-22.04-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 69f071c83d..fab7f3c1d1 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,8 +12,8 @@ jobs: name: Format runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 - - uses: actions/setup-python@v2 + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 with: python-version: '3.10' - name: Install system hooks diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 009c394bfe..5146786e8a 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -5,17 +5,19 @@ on: jobs: ament_lint: name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false matrix: linter: [cppcheck, copyright, lint_cmake] + env: + AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true steps: - - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 + - uses: actions/checkout@v4 + - uses: ros-tooling/setup-ros@v0.7 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: rolling + distribution: humble linter: ${{ matrix.linter }} package-name: controller_interface @@ -29,17 +31,17 @@ jobs: ament_lint_100: name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false matrix: linter: [cpplint] steps: - - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 + - uses: actions/checkout@v4 + - uses: ros-tooling/setup-ros@v0.7 - uses: ros-tooling/action-ros-lint@v0.1 with: - distribution: rolling + distribution: humble linter: cpplint arguments: "--linelength=100 --filter=-whitespace/newline" package-name: diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 3ea9a63541..f5015d8a90 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -19,13 +19,13 @@ jobs: steps: - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v1 + uses: docker/setup-buildx-action@v3 - name: Checkout repository - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Log in to the Container registry - uses: docker/login-action@v1 + uses: docker/login-action@v3 with: registry: ${{ env.REGISTRY }} username: ${{ github.actor }} @@ -33,7 +33,7 @@ jobs: - name: Docker meta id: meta - uses: docker/metadata-action@v3 + uses: docker/metadata-action@v5 with: images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}_release tags: | @@ -42,7 +42,7 @@ jobs: type=semver,pattern={{major}}.{{minor}} - name: Build and push Docker image - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v5 with: context: . push: true @@ -59,13 +59,13 @@ jobs: steps: - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v1 + uses: docker/setup-buildx-action@v3 - name: Checkout repository - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Log in to the Container registry - uses: docker/login-action@v1 + uses: docker/login-action@v3 with: registry: ${{ env.REGISTRY }} username: ${{ github.actor }} @@ -73,7 +73,7 @@ jobs: - name: Docker meta id: meta - uses: docker/metadata-action@v3 + uses: docker/metadata-action@v5 with: images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}_source tags: | @@ -82,7 +82,7 @@ jobs: type=semver,pattern={{major}}.{{minor}} - name: Build and push Docker image - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v5 with: context: . push: true diff --git a/.github/workflows/foxy-abi-compatibility.yml b/.github/workflows/foxy-abi-compatibility.yml index 7ce17effd0..6494e627a4 100644 --- a/.github/workflows/foxy-abi-compatibility.yml +++ b/.github/workflows/foxy-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: foxy diff --git a/.github/workflows/galactic-abi-compatibility.yml b/.github/workflows/galactic-abi-compatibility.yml index 06a48ef9c7..a5eb1e66c0 100644 --- a/.github/workflows/galactic-abi-compatibility.yml +++ b/.github/workflows/galactic-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: galactic diff --git a/.github/workflows/galactic-rhel-binary-build.yml b/.github/workflows/galactic-rhel-binary-build.yml index ca0b30382a..75927cc2ee 100644 --- a/.github/workflows/galactic-rhel-binary-build.yml +++ b/.github/workflows/galactic-rhel-binary-build.yml @@ -22,7 +22,7 @@ jobs: ROS_DISTRO: galactic container: ghcr.io/ros-controls/ros:galactic-rhel steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: src/ros2_control - run: | 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 989f46f24f..76b91a26a7 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/prerelease-check.yml b/.github/workflows/prerelease-check.yml index b9460bda47..809471897d 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -28,7 +28,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 490b680e7c..acefeebfac 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -58,22 +58,22 @@ 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 if: ${{ ! matrix.env.CCOV }} - uses: pat-s/always-upload-cache@v2.1.5 + uses: pat-s/always-upload-cache@v3.0.11 with: path: ${{ env.BASEDIR }}/target_ws key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} restore-keys: | target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} - name: cache ccache - uses: pat-s/always-upload-cache@v2.1.5 + uses: pat-s/always-upload-cache@v3.0.11 with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 4e5174405d..c90e3eec67 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,16 +26,17 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.3.4 + - uses: ros-tooling/setup-ros@v0.7 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.2.6 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package + ref: ${{ inputs.ref }} # otherwise the default branch is used for scheduled workflows package-name: controller_interface controller_manager @@ -49,7 +50,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@v1 + - uses: actions/upload-artifact@v4 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index f7ae929e5b..ed28964e01 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -6,8 +6,9 @@ on: jobs: test: runs-on: ubuntu-latest + if: github.actor != 'dependabot[bot]' && github.actor != 'mergify[bot]' steps: - - uses: actions/checkout@v1 - - uses: uesteibar/reviewer-lottery@v2 + - uses: actions/checkout@v4 + - uses: uesteibar/reviewer-lottery@v3 with: repo-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml index 4589e92e3b..3911434a23 100644 --- a/.github/workflows/rolling-abi-compatibility.yml +++ b/.github/workflows/rolling-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: rolling diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 563ad6e3d4..0af21bb1d8 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -22,7 +22,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: | diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 3d39d5f7ef..e2ecae645b 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.36.1 (2024-01-08) +------------------- + +2.36.0 (2023-12-12) +------------------- + +2.35.1 (2023-11-27) +------------------- + 2.35.0 (2023-11-14) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 348cd2691b..1fe84ff377 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 2.35.0 + 2.36.1 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 52a5192417..759074a096 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.36.1 (2024-01-08) +------------------- +* [docs] Remove joint_state_controller (`#1263 `_) (`#1264 `_) +* [CI] Increase timeout for controller_managers_srv test (backport `#1224 `_) (`#1225 `_) +* Contributors: mergify[bot] + +2.36.0 (2023-12-12) +------------------- +* Fix controller sorting issue while loading large number of controllers (`#1180 `_) (`#1186 `_) +* Contributors: mergify[bot] + +2.35.1 (2023-11-27) +------------------- + 2.35.0 (2023-11-14) ------------------- * Fix the controller sorting bug when the interfaces are not configured (fixes `#1164 `_) (`#1165 `_) (`#1166 `_) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index a3adf25ff7..29d489ad81 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -134,6 +134,7 @@ if(BUILD_TESTING) controller_manager_msgs ros2_control_test_assets ) + set_tests_properties(test_controller_manager_srvs PROPERTIES TIMEOUT 120) ament_add_gmock(test_controller_manager_urdf_passing test/test_controller_manager_urdf_passing.cpp ) diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index ee5cbdd3f0..849dee7fc0 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -20,6 +20,7 @@ list_hardware_interfaces, load_controller, reload_controller_libraries, + set_hardware_component_state, switch_controllers, unload_controller ) @@ -32,6 +33,7 @@ 'list_hardware_interfaces', 'load_controller', 'reload_controller_libraries', + 'set_hardware_component_state', 'switch_controllers', 'unload_controller', ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 8fb2c1aca7..91c7ab7517 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -12,9 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager_msgs.srv import ConfigureController, \ - ListControllers, ListControllerTypes, ListHardwareComponents, ListHardwareInterfaces, \ - LoadController, ReloadControllerLibraries, SwitchController, UnloadController +from controller_manager_msgs.srv import ( + ConfigureController, + ListControllers, + ListControllerTypes, + ListHardwareComponents, + ListHardwareInterfaces, + LoadController, + ReloadControllerLibraries, + SetHardwareComponentState, + SwitchController, + UnloadController, +) import rclpy @@ -37,55 +46,107 @@ def service_caller(node, service_name, service_type, request, service_timeout=10 raise RuntimeError(f'Exception while calling service: {future.exception()}') -def configure_controller(node, controller_manager_name, controller_name): +def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = ConfigureController.Request() request.name = controller_name - return service_caller(node, f'{controller_manager_name}/configure_controller', - ConfigureController, request) + return service_caller( + node, + f"{controller_manager_name}/configure_controller", + ConfigureController, + request, + service_timeout, + ) -def list_controllers(node, controller_manager_name): +def list_controllers(node, controller_manager_name, service_timeout=10.0): request = ListControllers.Request() - return service_caller(node, f'{controller_manager_name}/list_controllers', - ListControllers, request) + return service_caller( + node, + f"{controller_manager_name}/list_controllers", + ListControllers, + request, + service_timeout, + ) -def list_controller_types(node, controller_manager_name): +def list_controller_types(node, controller_manager_name, service_timeout=10.0): request = ListControllerTypes.Request() - return service_caller(node, - f'{controller_manager_name}/list_controller_types', - ListControllerTypes, request) + return service_caller( + node, + f"{controller_manager_name}/list_controller_types", + ListControllerTypes, + request, + service_timeout, + ) -def list_hardware_components(node, controller_manager_name): +def list_hardware_components(node, controller_manager_name, service_timeout=10.0): request = ListHardwareComponents.Request() - return service_caller(node, f'{controller_manager_name}/list_hardware_components', - ListHardwareComponents, request) + return service_caller( + node, + f"{controller_manager_name}/list_hardware_components", + ListHardwareComponents, + request, + service_timeout, + ) -def list_hardware_interfaces(node, controller_manager_name): +def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0): request = ListHardwareInterfaces.Request() - return service_caller(node, f'{controller_manager_name}/list_hardware_interfaces', - ListHardwareInterfaces, request) + return service_caller( + node, + f"{controller_manager_name}/list_hardware_interfaces", + ListHardwareInterfaces, + request, + service_timeout, + ) -def load_controller(node, controller_manager_name, controller_name): +def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = LoadController.Request() request.name = controller_name - return service_caller(node, f'{controller_manager_name}/load_controller', - LoadController, request) + return service_caller( + node, + f"{controller_manager_name}/load_controller", + LoadController, + request, + service_timeout, + ) -def reload_controller_libraries(node, controller_manager_name, force_kill): +def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0): request = ReloadControllerLibraries.Request() request.force_kill = force_kill - return service_caller(node, - f'{controller_manager_name}/reload_controller_libraries', - ReloadControllerLibraries, request) - - -def switch_controllers(node, controller_manager_name, deactivate_controllers, - activate_controllers, strict, activate_asap, timeout): + return service_caller( + node, + f"{controller_manager_name}/reload_controller_libraries", + ReloadControllerLibraries, + request, + service_timeout, + ) + + +def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state): + request = SetHardwareComponentState.Request() + request.name = component_name + request.target_state = lifecyle_state + return service_caller( + node, + f"{controller_manager_name}/set_hardware_component_state", + SetHardwareComponentState, + request, + ) + + +def switch_controllers( + node, + controller_manager_name, + deactivate_controllers, + activate_controllers, + strict, + activate_asap, + timeout, +): request = SwitchController.Request() request.activate_controllers = activate_controllers request.deactivate_controllers = deactivate_controllers @@ -99,8 +160,13 @@ def switch_controllers(node, controller_manager_name, deactivate_controllers, SwitchController, request) -def unload_controller(node, controller_manager_name, controller_name): +def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = UnloadController.Request() request.name = controller_name - return service_caller(node, f'{controller_manager_name}/unload_controller', - UnloadController, request) + return service_caller( + node, + f"{controller_manager_name}/unload_controller", + UnloadController, + request, + service_timeout, + ) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py new file mode 100644 index 0000000000..c95fb6181e --- /dev/null +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import sys +import time + +from controller_manager import set_hardware_component_state + +from lifecycle_msgs.msg import State +import rclpy +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.signals import SignalHandlerOptions + + +# from https://stackoverflow.com/a/287944 +class bcolors: + HEADER = "\033[95m" + OKBLUE = "\033[94m" + OKCYAN = "\033[96m" + OKGREEN = "\033[92m" + WARNING = "\033[93m" + FAIL = "\033[91m" + ENDC = "\033[0m" + BOLD = "\033[1m" + UNDERLINE = "\033[4m" + + +def first_match(iterable, predicate): + return next((n for n in iterable if predicate(n)), None) + + +def wait_for_value_or(function, node, timeout, default, description): + while node.get_clock().now() < timeout: + if result := function(): + return result + node.get_logger().info( + f"Waiting for {description}", throttle_duration_sec=2, skip_first=True + ) + time.sleep(0.2) + return default + + +def combine_name_and_namespace(name_and_namespace): + node_name, namespace = name_and_namespace + return namespace + ("" if namespace.endswith("/") else "/") + node_name + + +def find_node_and_namespace(node, full_node_name): + node_names_and_namespaces = node.get_node_names_and_namespaces() + return first_match( + node_names_and_namespaces, + lambda n: combine_name_and_namespace(n) == full_node_name, + ) + + +def has_service_names(node, node_name, node_namespace, service_names): + client_names_and_types = node.get_service_names_and_types_by_node(node_name, node_namespace) + if not client_names_and_types: + return False + client_names, _ = zip(*client_names_and_types) + return all(service in client_names for service in service_names) + + +def wait_for_controller_manager(node, controller_manager, timeout_duration): + # List of service names from controller_manager we wait for + service_names = ( + f"{controller_manager}/list_hardware_components", + f"{controller_manager}/set_hardware_component_state", + ) + + # Wait for controller_manager + timeout = node.get_clock().now() + Duration(seconds=timeout_duration) + node_and_namespace = wait_for_value_or( + lambda: find_node_and_namespace(node, controller_manager), + node, + timeout, + None, + f"'{controller_manager}' node to exist", + ) + + # Wait for the services if the node was found + if node_and_namespace: + node_name, namespace = node_and_namespace + return wait_for_value_or( + lambda: has_service_names(node, node_name, namespace, service_names), + node, + timeout, + False, + f"'{controller_manager}' services to be available", + ) + + return False + + +def handle_set_component_state_service_call( + node, controller_manager_name, component, target_state, action +): + response = set_hardware_component_state(node, controller_manager_name, component, target_state) + if response.ok and response.state == target_state: + node.get_logger().info( + bcolors.OKGREEN + + f"{action} component '{component}'. Hardware now in state: {response.state}." + ) + elif response.ok and not response.state == target_state: + node.get_logger().warn( + bcolors.WARNING + + f"Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'." + ) + else: + node.get_logger().warn( + bcolors.WARNING + + f"Could not {action} component '{component}'. Service call failed. Wrong component name?" + ) + + +def activate_components(node, controller_manager_name, components_to_activate): + active_state = State() + active_state.id = State.PRIMARY_STATE_ACTIVE + active_state.label = "active" + for component in components_to_activate: + handle_set_component_state_service_call( + node, controller_manager_name, component, active_state, "activated" + ) + + +def configure_components(node, controller_manager_name, components_to_configure): + inactive_state = State() + inactive_state.id = State.PRIMARY_STATE_INACTIVE + inactive_state.label = "inactive" + for component in components_to_configure: + handle_set_component_state_service_call( + node, controller_manager_name, component, inactive_state, "configured" + ) + + +def main(args=None): + rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) + parser = argparse.ArgumentParser() + activate_or_confiigure_grp = parser.add_mutually_exclusive_group(required=True) + + parser.add_argument( + "hardware_component_name", + help="The name of the hardware component which should be activated.", + ) + parser.add_argument( + "-c", + "--controller-manager", + help="Name of the controller manager ROS node", + default="controller_manager", + required=False, + ) + parser.add_argument( + "--controller-manager-timeout", + help="Time to wait for the controller manager", + required=False, + default=10, + type=int, + ) + + # add arguments which are mutually exclusive + activate_or_confiigure_grp.add_argument( + "--activate", + help="Activates the given components. Note: Components are by default configured before activated. ", + action="store_true", + required=False, + ) + activate_or_confiigure_grp.add_argument( + "--configure", + help="Configures the given components.", + action="store_true", + required=False, + ) + + command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] + args = parser.parse_args(command_line_args) + controller_manager_name = args.controller_manager + controller_manager_timeout = args.controller_manager_timeout + hardware_component = [args.hardware_component_name] + activate = args.activate + configure = args.configure + + node = Node("hardware_spawner") + if not controller_manager_name.startswith("/"): + spawner_namespace = node.get_namespace() + if spawner_namespace != "/": + controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" + else: + controller_manager_name = f"/{controller_manager_name}" + + try: + if not wait_for_controller_manager( + node, controller_manager_name, controller_manager_timeout + ): + node.get_logger().error("Controller manager not available") + return 1 + + if activate: + activate_components(node, controller_manager_name, hardware_component) + elif configure: + configure_components(node, controller_manager_name, hardware_component) + else: + node.get_logger().error( + 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' + ) + parser.print_help() + return 0 + finally: + rclpy.shutdown() + + +if __name__ == "__main__": + ret = main() + sys.exit(ret) diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index d3f9356eae..e9adfaa50f 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -32,12 +32,12 @@ def generate_load_controller_launch_description(controller_name, Examples # noqa: D416 -------- # Assuming the controller type and controller parameters are known to the controller_manager - generate_load_controller_launch_description('joint_state_controller') + generate_load_controller_launch_description('joint_state_broadcaster') # Passing controller type and controller parameter file to load generate_load_controller_launch_description( - 'joint_state_controller', - controller_type='joint_state_controller/JointStateController', + 'joint_state_broadcaster', + controller_type='joint_state_broadcaster/JointStateBroadcaster', controller_params_file=os.path.join(get_package_share_directory('my_pkg'), 'config', 'controller_params.yaml') ) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index db8ea211e5..28a14af5fd 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 2.35.0 + 2.36.1 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager/setup.cfg b/controller_manager/setup.cfg index ef02ee65bd..92c5581e92 100644 --- a/controller_manager/setup.cfg +++ b/controller_manager/setup.cfg @@ -2,3 +2,4 @@ console_scripts = spawner = controller_manager.spawner:main unspawner = controller_manager.unspawner:main + hardware_spawner = controller_manager.hardware_spawner:main diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a4ff3dfef1..18b09063e7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -284,9 +284,8 @@ ControllerManager::ControllerManager( "[Deprecated] Passing the robot description parameter directly to the control_manager node " "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead."); init_resource_manager(robot_description); + init_services(); } - - init_services(); } ControllerManager::ControllerManager( @@ -311,7 +310,10 @@ ControllerManager::ControllerManager( { subscribe_to_robot_description_topic(); } - init_services(); + else + { + init_services(); + } } void ControllerManager::subscribe_to_robot_description_topic() @@ -343,6 +345,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & return; } init_resource_manager(robot_description.data.c_str()); + init_services(); } catch (std::runtime_error & e) { @@ -401,14 +404,14 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED)); // inactive (configured) - // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023) + // BEGIN: Keep old functionality on for backwards compatibility std::vector configure_components_on_start = std::vector({}); get_parameter("configure_components_on_start", configure_components_on_start); if (!configure_components_on_start.empty()) { RCLCPP_WARN( get_logger(), - "Parameter 'configure_components_on_start' is deprecated. " + "[Deprecated]: Parameter 'configure_components_on_start' is deprecated. " "Use 'hardware_interface_state_after_start.inactive' instead, to set component's initial " "state to 'inactive'. Don't use this parameters in combination with the new " "'hardware_interface_state_after_start' parameter structure."); @@ -426,18 +429,16 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); } - // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023) + // BEGIN: Keep old functionality on for backwards compatibility std::vector activate_components_on_start = std::vector({}); get_parameter("activate_components_on_start", activate_components_on_start); - rclcpp_lifecycle::State active_state( - State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); if (!activate_components_on_start.empty()) { RCLCPP_WARN( get_logger(), - "Parameter 'activate_components_on_start' is deprecated. " + "[Deprecated]: Parameter 'activate_components_on_start' is deprecated. " "Components are activated per default. Don't use this parameters in combination with the new " - "'hardware_interface_state_after_start' parameter structure."); + "'hardware_components_initial_state' parameter structure."); rclcpp_lifecycle::State active_state( State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); for (const auto & component : activate_components_on_start) @@ -763,7 +764,7 @@ controller_interface::return_type ControllerManager::configure_controller( to = from; // Reordering the controllers - std::sort( + std::stable_sort( to.begin(), to.end(), std::bind( &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, @@ -2363,7 +2364,16 @@ bool ControllerManager::controller_sorting( { // The case of the controllers that don't have any command interfaces. For instance, // joint_state_broadcaster - return true; + // If the controller b is also under the same condition, then maintain their initial order + if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) + return false; + else + return true; + } + else if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) + { + // If only the controller b is a broadcaster or non chainable type , then swap the controllers + return false; } else { diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index e598165eab..30849a3ba3 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -1008,3 +1008,499 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) // third tree ASSERT_GT(ctrl_chain_7_pos, ctrl_chain_8_pos); } + +TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) +{ + /// The simulated controller chaining is: + /// test_controller_name_1 -> chain_ctrl_3 -> chain_ctrl_2 -> chain_ctrl_1 + /// && + /// test_controller_name_2 -> chain_ctrl_6 -> chain_ctrl_5 -> chain_ctrl_4 + /// && + /// test_controller_name_7 -> test_controller_name_8 + /// && + /// There are 100 more other basic controllers and 100 more different broadcasters to check for + /// crashing + /// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces + /// exported from the controller B (or) the controller B is utilizing the expected interfaces + /// exported from the controller A + + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create set of chained controllers + static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1"; + static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2"; + static constexpr char TEST_CHAINED_CONTROLLER_3[] = "test_chainable_controller_name_3"; + static constexpr char TEST_CHAINED_CONTROLLER_4[] = "test_chainable_controller_name_4"; + static constexpr char TEST_CHAINED_CONTROLLER_5[] = "test_chainable_controller_name_5"; + static constexpr char TEST_CHAINED_CONTROLLER_6[] = "test_chainable_controller_name_6"; + static constexpr char TEST_CHAINED_CONTROLLER_7[] = "test_chainable_controller_name_7"; + static constexpr char TEST_CHAINED_CONTROLLER_8[] = "test_chainable_controller_name_8"; + static constexpr char TEST_CHAINED_CONTROLLER_9[] = "test_chainable_controller_name_9"; + static constexpr char TEST_CONTROLLER_1[] = "test_controller_name_1"; + static constexpr char TEST_CONTROLLER_2[] = "test_controller_name_2"; + + // First chain + auto test_chained_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_1->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_2 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}}; + test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_2->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_3 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position"}}; + test_chained_controller_3->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_3->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_3->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_3) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_3) + "/joint1/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_1->set_command_interface_configuration(cmd_cfg); + test_controller_1->set_state_interface_configuration(state_cfg); + + // Second chain + auto test_chained_controller_4 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}}; + test_chained_controller_4->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_4->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_4->set_reference_interface_names({"joint2/velocity"}); + + auto test_chained_controller_5 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_4) + "/joint2/velocity"}}; + test_chained_controller_5->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_5->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_5->set_reference_interface_names({"joint2/velocity"}); + + auto test_chained_controller_6 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_5) + "/joint2/velocity"}}; + test_chained_controller_6->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_6->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_6->set_reference_interface_names({"joint2/velocity"}); + + auto test_controller_2 = std::make_shared(); + cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_6) + "/joint2/velocity"}}; + state_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}}; + test_controller_2->set_command_interface_configuration(cmd_cfg); + test_controller_2->set_state_interface_configuration(state_cfg); + + // Third chain + auto test_chained_controller_7 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint3/velocity"}}; + test_chained_controller_7->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_7->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_7->set_reference_interface_names({"joint3/velocity"}); + + auto test_chained_controller_8 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_7) + "/joint3/velocity"}}; + test_chained_controller_8->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_8->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_8->set_reference_interface_names({"joint3/velocity"}); + + auto test_chained_controller_9 = std::make_shared(); + cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/max_acceleration"}}; + test_chained_controller_9->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_9->set_state_interface_configuration(state_cfg); + + unsigned int num_of_random_broadcasters = 100; + unsigned int num_of_random_controllers = 100; + std::vector chained_ref_interfaces; + for (size_t i = 0; i < num_of_random_controllers; i++) + { + chained_ref_interfaces.push_back("ref_" + std::to_string(i) + "/joint_2/acceleration"); + } + test_chained_controller_9->set_reference_interface_names(chained_ref_interfaces); + std::unordered_map> random_controllers_list; + for (size_t i = 0; i < num_of_random_broadcasters; i++) + { + auto controller_name = "test_broadcaster_" + std::to_string(i); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + } + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_controllers_" + std::to_string(i); + RCLCPP_ERROR(srv_node->get_logger(), "Initializing controller : %s !", controller_name.c_str()); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + random_controllers_list[controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_9) + std::string("/ref_") + std::to_string(i) + + std::string("/joint_2/acceleration")}}); + } + + // add controllers + /// @todo add controllers in random order + /// For now, adding the ordered case to see that current sorting doesn't change order + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_6, TEST_CHAINED_CONTROLLER_6, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_8, TEST_CHAINED_CONTROLLER_8, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_9, TEST_CHAINED_CONTROLLER_9, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + { + ControllerManagerRunner cm_runner(this); + for (auto random_ctrl : random_controllers_list) + { + cm_->add_controller( + random_ctrl.second, random_ctrl.first, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + } + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + + // check chainable controller + ASSERT_EQ( + 11u + num_of_random_broadcasters + num_of_random_controllers, result->controller.size()); + EXPECT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_2); + EXPECT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_6); + EXPECT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1); + EXPECT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_7); + EXPECT_EQ(result->controller[4].name, TEST_CONTROLLER_1); + + EXPECT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_5); + EXPECT_EQ(result->controller[6].name, TEST_CHAINED_CONTROLLER_3); + EXPECT_EQ(result->controller[7].name, TEST_CHAINED_CONTROLLER_4); + EXPECT_EQ(result->controller[8].name, TEST_CONTROLLER_2); + EXPECT_EQ(result->controller[9].name, TEST_CHAINED_CONTROLLER_8); + + // configure controllers + auto ctrls_order = { + TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_9, + TEST_CHAINED_CONTROLLER_1, TEST_CONTROLLER_1, TEST_CHAINED_CONTROLLER_4, + TEST_CONTROLLER_2, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_6, + TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_8}; + { + ControllerManagerRunner cm_runner(this); + for (const auto & controller : ctrls_order) + { + cm_->configure_controller(controller); + } + + for (auto random_ctrl : random_controllers_list) + { + cm_->configure_controller(random_ctrl.first); + } + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 11u + num_of_random_broadcasters + num_of_random_controllers, result->controller.size()); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t + { + auto it = std::find_if( + result->controller.begin(), result->controller.end(), + [controller_name](auto itf) + { return (itf.name.find(controller_name) != std::string::npos); }); + return std::distance(result->controller.begin(), it); + }; + auto ctrl_chain_1_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_1); + auto ctrl_chain_2_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_2); + auto ctrl_chain_3_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_3); + auto ctrl_chain_4_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_4); + auto ctrl_chain_5_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_5); + auto ctrl_chain_6_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_6); + auto ctrl_chain_7_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_7); + auto ctrl_chain_8_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_8); + auto ctrl_chain_9_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_9); + + auto ctrl_1_pos = get_ctrl_pos(TEST_CONTROLLER_1); + auto ctrl_2_pos = get_ctrl_pos(TEST_CONTROLLER_2); + + // Extra check to see that they are indexed after their parent controller + // first chain + ASSERT_GT(ctrl_chain_1_pos, ctrl_chain_2_pos); + ASSERT_GT(ctrl_chain_2_pos, ctrl_chain_3_pos); + ASSERT_GT(ctrl_chain_3_pos, ctrl_1_pos); + + // second tree + ASSERT_GT(ctrl_chain_4_pos, ctrl_chain_5_pos); + ASSERT_GT(ctrl_chain_5_pos, ctrl_chain_6_pos); + ASSERT_GT(ctrl_chain_6_pos, ctrl_2_pos); + + // third tree + ASSERT_GT(ctrl_chain_7_pos, ctrl_chain_8_pos); + + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_controllers_" + std::to_string(i); + ASSERT_GT(ctrl_chain_9_pos, get_ctrl_pos(controller_name)); + } + RCLCPP_INFO(srv_node->get_logger(), "Check successful!"); +} + +TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) +{ + /// The simulated controller chain is like every joint has its own controller exposing interfaces + /// and then a controller chain using those interfaces + /// + /// There are 20 more broadcasters + 20 more normal controllers for complexity + /// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported + /// from the controller B (or) the controller B is utilizing the expected interfaces exported from + /// the controller A + + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + const unsigned int joints_count = 20; + + static constexpr char JOINT_CONTROLLER_PREFIX[] = "test_chainable_controller_name_joint_"; + static constexpr char FWD_CONTROLLER_PREFIX[] = "forward_controller_joint_"; + static constexpr char JOINT_SENSOR_BROADCASTER_PREFIX[] = "test_broadcaster_joint_"; + std::vector controllers_list; + std::vector fwd_joint_position_interfaces_list; + std::vector fwd_joint_velocity_interfaces_list; + std::vector fwd_joint_position_ref_interfaces_list; + std::vector fwd_joint_velocity_ref_interfaces_list; + std::unordered_map> + random_chainable_controllers_list; + std::unordered_map> random_controllers_list; + for (size_t i = 0; i < joints_count; i++) + { + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/position", "joint" + std::to_string(i) + "/velocity"}}; + // Joint controller + const std::string controller_name = JOINT_CONTROLLER_PREFIX + std::to_string(i); + random_chainable_controllers_list[controller_name] = + std::make_shared(); + random_chainable_controllers_list[controller_name]->set_state_interface_configuration( + chained_state_cfg); + random_chainable_controllers_list[controller_name]->set_command_interface_configuration( + chained_cmd_cfg); + random_chainable_controllers_list[controller_name]->set_reference_interface_names( + chained_state_cfg.names); + controllers_list.push_back(controller_name); + + // Forward Joint interfaces controller + fwd_joint_position_interfaces_list.push_back( + std::string(controller_name) + "/joint" + std::to_string(i) + "/position"); + fwd_joint_velocity_interfaces_list.push_back( + std::string(controller_name) + "/joint" + std::to_string(i) + "/velocity"); + const std::string fwd_controller_name = FWD_CONTROLLER_PREFIX + std::to_string(i); + random_chainable_controllers_list[fwd_controller_name] = + std::make_shared(); + random_chainable_controllers_list[fwd_controller_name]->set_state_interface_configuration( + chained_state_cfg); + random_chainable_controllers_list[fwd_controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {fwd_joint_position_interfaces_list.back(), fwd_joint_velocity_interfaces_list.back()}}); + random_chainable_controllers_list[fwd_controller_name]->set_reference_interface_names( + chained_state_cfg.names); + fwd_joint_position_ref_interfaces_list.push_back( + std::string(fwd_controller_name) + "/" + fwd_joint_position_interfaces_list.back()); + fwd_joint_velocity_ref_interfaces_list.push_back( + std::string(fwd_controller_name) + "/" + fwd_joint_velocity_interfaces_list.back()); + controllers_list.push_back(fwd_controller_name); + + // Add a broadcaster for every joint assuming it as a sensor (just for the tests) + const std::string broadcaster_name = JOINT_SENSOR_BROADCASTER_PREFIX + std::to_string(i); + random_controllers_list[broadcaster_name] = std::make_shared(); + random_controllers_list[broadcaster_name]->set_state_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/torque", "joint" + std::to_string(i) + "/torque"}}); + controllers_list.push_back(broadcaster_name); + } + + // create set of chained controllers + static constexpr char POSITION_REFERENCE_CONTROLLER[] = "position_reference_chainable_controller"; + static constexpr char VELOCITY_REFERENCE_CONTROLLER[] = "velocity_reference_chainable_controller"; + static constexpr char HIGHER_LEVEL_REFERENCE_CONTROLLER[] = "task_level_controller"; + + // Position reference controller + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER] = + std::make_shared(); + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER] + ->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + fwd_joint_position_ref_interfaces_list}); + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER]->set_reference_interface_names( + {"joint/position"}); + controllers_list.push_back(POSITION_REFERENCE_CONTROLLER); + + // Velocity reference controller + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER] = + std::make_shared(); + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER] + ->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + fwd_joint_velocity_ref_interfaces_list}); + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER]->set_reference_interface_names( + {"joint/velocity"}); + controllers_list.push_back(VELOCITY_REFERENCE_CONTROLLER); + + // Higher level task level controller + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER] = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(POSITION_REFERENCE_CONTROLLER) + "/joint/position", + std::string(VELOCITY_REFERENCE_CONTROLLER) + "/joint/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER]->set_command_interface_configuration( + cmd_cfg); + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER]->set_state_interface_configuration( + state_cfg); + controllers_list.push_back(HIGHER_LEVEL_REFERENCE_CONTROLLER); + + const unsigned int num_of_random_broadcasters = 20; + const unsigned int num_of_random_controllers = 20; + for (size_t i = 0; i < num_of_random_broadcasters; i++) + { + auto controller_name = "test_broadcaster_" + std::to_string(i); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + controllers_list.push_back(controller_name); + } + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_reference_controllers_" + std::to_string(i); + RCLCPP_ERROR(srv_node->get_logger(), "Initializing controller : %s !", controller_name.c_str()); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + random_controllers_list[controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string("ref_") + std::to_string(i) + std::string("/joint_2/acceleration")}}); + controllers_list.push_back(controller_name); + } + + // Now shuffle the list to be able to configure controller later randomly + std::random_shuffle(controllers_list.begin(), controllers_list.end()); + + { + ControllerManagerRunner cm_runner(this); + for (auto random_chain_ctrl : random_chainable_controllers_list) + { + cm_->add_controller( + random_chain_ctrl.second, random_chain_ctrl.first, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + for (auto random_ctrl : random_controllers_list) + { + cm_->add_controller( + random_ctrl.second, random_ctrl.first, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + } + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + + // check chainable controller + ASSERT_EQ(controllers_list.size(), result->controller.size()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + for (auto random_ctrl : controllers_list) + { + cm_->configure_controller(random_ctrl); + } + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(controllers_list.size(), result->controller.size()); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t + { + auto it = std::find_if( + result->controller.begin(), result->controller.end(), + [controller_name](auto itf) + { return (itf.name.find(controller_name) != std::string::npos); }); + return std::distance(result->controller.begin(), it); + }; + + // Check the controller indexing + auto pos_ref_pos = get_ctrl_pos(POSITION_REFERENCE_CONTROLLER); + auto vel_ref_pos = get_ctrl_pos(VELOCITY_REFERENCE_CONTROLLER); + auto task_level_ctrl_pos = get_ctrl_pos(HIGHER_LEVEL_REFERENCE_CONTROLLER); + ASSERT_GT(vel_ref_pos, task_level_ctrl_pos); + ASSERT_GT(pos_ref_pos, task_level_ctrl_pos); + + for (size_t i = 0; i < joints_count; i++) + { + const std::string controller_name = JOINT_CONTROLLER_PREFIX + std::to_string(i); + const std::string fwd_controller_name = FWD_CONTROLLER_PREFIX + std::to_string(i); + + ASSERT_GT(get_ctrl_pos(fwd_controller_name), pos_ref_pos); + ASSERT_GT(get_ctrl_pos(fwd_controller_name), vel_ref_pos); + ASSERT_GT(get_ctrl_pos(controller_name), pos_ref_pos); + ASSERT_GT(get_ctrl_pos(controller_name), vel_ref_pos); + } + RCLCPP_INFO(srv_node->get_logger(), "Check successful!"); +} diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index c4c9b8bb67..17402c1f83 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -84,7 +84,9 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs "Unable to initialize resource manager, no robot description found."); } - cm_->init_resource_manager(robot_description); + auto msg = std_msgs::msg::String(); + msg.data = robot_description; + cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); } @@ -202,6 +204,38 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs } }; +class TestControllerManagerHWManagementSrvsWithoutParams +: public TestControllerManagerHWManagementSrvs +{ +public: + void SetUp() override + { + executor_ = std::make_shared(); + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + run_updater_ = false; + + // TODO(destogl): separate this to init_tests method where parameter can be set for each test + // separately + cm_->set_parameter( + rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); + + std::string robot_description = ""; + cm_->get_parameter("robot_description", robot_description); + if (robot_description.empty()) + { + throw std::runtime_error( + "Unable to initialize resource manager, no robot description found."); + } + + auto msg = std_msgs::msg::String(); + msg.data = robot_description; + cm_->robot_description_callback(msg); + + SetUpSrvsCMExecutor(); + } +}; + TEST_F(TestControllerManagerHWManagementSrvs, list_hardware_components) { // Default status after start: @@ -413,7 +447,7 @@ TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activati })); } -// BEGIN: Remove at the end of 2023 +// BEGIN: Deprecated parameters class TestControllerManagerHWManagementSrvsOldParameters : public TestControllerManagerHWManagementSrvs { @@ -470,4 +504,4 @@ TEST_F(TestControllerManagerHWManagementSrvsOldParameters, list_hardware_compone {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } -// END: Remove at the end of 2023 +// END: Deprecated parameters diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 133904dc45..85c6294eff 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.36.1 (2024-01-08) +------------------- + +2.36.0 (2023-12-12) +------------------- + +2.35.1 (2023-11-27) +------------------- + 2.35.0 (2023-11-14) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index a154e28995..43438f151f 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 2.35.0 + 2.36.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 be6c3e2ff8..27e61ca36a 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.36.1 (2024-01-08) +------------------- +* [ResourceManager] adds test for uninitialized hardware (`#1243 `_) (`#1274 `_) +* Use portable version for string-to-double conversion (backport `#1257 `_) (`#1268 `_) +* Fix typo in docs (`#1219 `_) (`#1221 `_) +* Contributors: Christoph Fröhlich, mergify[bot] + +2.36.0 (2023-12-12) +------------------- +* Cleanup Resource Manager a bit to increase clarity. (backport `#816 `_) (`#1191 `_) +* Handle hardware errors in Resource Manager (`#805 `_) (`#837 `_) #ABI-breaking +* Contributors: mergify[bot] + +2.35.1 (2023-11-27) +------------------- +* [MockHardware] Fix the issues where hardware with multiple interfaces can not be started because of a logical bug added when adding dynamics calculation functionality. (`#1151 `_) (`#1178 `_) +* Contributors: Dr Denis + 2.35.0 (2023-11-14) ------------------- * [CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (backport `#940 `_) (`#1052 `_) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 2fb31641b0..aa7ca933ff 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -29,6 +29,7 @@ add_library( src/resource_manager.cpp src/sensor.cpp src/system.cpp + src/lexical_casts.cpp ) target_include_directories( ${PROJECT_NAME} diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index fb527d39d2..326b3dfd2a 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -14,7 +14,7 @@ The main intention is to reduce debugging time on the physical hardware and boos Generic System ^^^^^^^^^^^^^^ -The component implements ``hardware_interface::SystemInterface>`` supporting command and state interfaces. +The component implements ``hardware_interface::SystemInterface`` supporting command and state interfaces. For more information about hardware components check :ref:`detailed documentation `. Features: diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 5112f7927e..d5d999cca8 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -33,8 +33,5 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); -HARDWARE_INTERFACE_PUBLIC -bool parse_bool(const std::string & bool_string); - } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/lexical_casts.hpp b/hardware_interface/include/hardware_interface/lexical_casts.hpp new file mode 100644 index 0000000000..1b9bad7018 --- /dev/null +++ b/hardware_interface/include/hardware_interface/lexical_casts.hpp @@ -0,0 +1,37 @@ +// Copyright 2023 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ +#define HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ + +#include +#include +#include +#include + +namespace hardware_interface +{ + +/** \brief Helper function to convert a std::string to double in a locale-independent way. + \throws std::invalid_argument if not a valid number + * from + https://github.com/ros-planning/srdfdom/blob/ad17b8d25812f752c397a6011cec64aeff090c46/src/model.cpp#L53 +*/ +double stod(const std::string & s); + +bool parse_bool(const std::string & bool_string); + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 40b21cd567..8cce2a5c0b 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -36,6 +36,12 @@ class SensorInterface; class SystemInterface; class ResourceStorage; +struct HardwareReadWriteStatus +{ + bool ok; + std::vector failed_hardware_names; +}; + class HARDWARE_INTERFACE_PUBLIC ResourceManager { public: @@ -111,12 +117,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ std::vector available_state_interfaces() const; - /// Checks whether a state interface is registered under the given key. - /** - * \return true if interface exist, false otherwise. - */ - bool state_interface_exists(const std::string & key) const; - /// Checks whether a state interface is available under the given key. /** * \return true if interface is available, false otherwise. @@ -175,6 +175,27 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void remove_controller_reference_interfaces(const std::string & controller_name); + /// Cache mapping between hardware and controllers using it + /** + * Find mapping between controller and hardware based on interfaces controller with + * \p controller_name is using and cache those for later usage. + * + * \param[in] controller_name name of the controller which interfaces are provided. + * \param[in] interfaces list of interfaces controller with \p controller_name is using. + */ + void cache_controller_to_hardware( + const std::string & controller_name, const std::vector & interfaces); + + /// Return cached controllers for a specific hardware. + /** + * Return list of cached controller names that use the hardware with name \p hardware_name. + * + * \param[in] hardware_name the name of the hardware for which cached controllers should be + * returned. \returns list of cached controller names that depend on hardware with name \p + * hardware_name. + */ + std::vector get_cached_controllers_to_hardware(const std::string & hardware_name); + /// Checks whether a command interface is already claimed. /** * Any command interface can only be claimed by a single instance. @@ -211,13 +232,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ std::vector available_command_interfaces() const; - /// Checks whether a command interface is registered under the given key. - /** - * \param[in] key string identifying the interface to check. - * \return true if interface exist, false otherwise. - */ - bool command_interface_exists(const std::string & key) const; - /// Checks whether a command interface is available under the given name. /** * \param[in] name string identifying the interface to check. @@ -354,7 +368,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * Part of the real-time critical update loop. * It is realtime-safe if used hardware interfaces are implemented adequately. */ - void read(const rclcpp::Time & time, const rclcpp::Duration & period); + HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period); /// Write all loaded hardware components. /** @@ -363,7 +377,20 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * Part of the real-time critical update loop. * It is realtime-safe if used hardware interfaces are implemented adequately. */ - void write(const rclcpp::Time & time, const rclcpp::Duration & period); + HardwareReadWriteStatus write(const rclcpp::Time & time, const rclcpp::Duration & period); + + /// Checks whether a command interface is registered under the given key. + /** + * \param[in] key string identifying the interface to check. + * \return true if interface exist, false otherwise. + */ + bool command_interface_exists(const std::string & key) const; + + /// Checks whether a state interface is registered under the given key. + /** + * \return true if interface exist, false otherwise. + */ + bool state_interface_exists(const std::string & key) const; /// Checks whether a command interface is registered under the given key. /** @@ -387,8 +414,13 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager mutable std::recursive_mutex resource_interfaces_lock_; mutable std::recursive_mutex claimed_command_interfaces_lock_; + mutable std::recursive_mutex resources_lock_; + std::unique_ptr resource_storage_; + // Structure to store read and write status so it is not initialized in the real-time loop + HardwareReadWriteStatus read_write_status; + bool is_urdf_loaded__ = false; }; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 3c95528156..e28969a8a4 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 2.35.0 + 2.36.1 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 3766c03d79..6b58e365dc 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -218,6 +218,12 @@ const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -234,6 +240,12 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index c05ec8ca47..da83d69bee 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -23,6 +23,7 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/lexical_casts.hpp" namespace { @@ -135,7 +136,7 @@ double get_parameter_value_or( const auto tag_text = params_it->GetText(); if (tag_text) { - return std::stod(tag_text); + return hardware_interface::stod(tag_text); } } } @@ -593,9 +594,4 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } -bool parse_bool(const std::string & bool_string) -{ - return bool_string == "true" || bool_string == "True"; -} - } // namespace hardware_interface diff --git a/hardware_interface/src/lexical_casts.cpp b/hardware_interface/src/lexical_casts.cpp new file mode 100644 index 0000000000..c9adcccf83 --- /dev/null +++ b/hardware_interface/src/lexical_casts.cpp @@ -0,0 +1,37 @@ +// Copyright 2024 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "hardware_interface/lexical_casts.hpp" + +namespace hardware_interface +{ +double stod(const std::string & s) +{ + // convert from string using no locale + std::istringstream stream(s); + stream.imbue(std::locale::classic()); + double result; + stream >> result; + if (stream.fail() || !stream.eof()) + { + throw std::invalid_argument("Failed converting string to real number"); + } + return result; +} + +bool parse_bool(const std::string & bool_string) +{ + return bool_string == "true" || bool_string == "True"; +} +} // namespace hardware_interface diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index f34c08c9a4..0a88be99cd 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -25,6 +25,7 @@ #include #include "hardware_interface/component_parser.hpp" +#include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rcutils/logging_macros.h" @@ -136,7 +137,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i it = info_.hardware_parameters.find("position_state_following_offset"); if (it != info_.hardware_parameters.end()) { - position_state_following_offset_ = std::stod(it->second); + position_state_following_offset_ = hardware_interface::stod(it->second); it = info_.hardware_parameters.find("custom_interface_with_following_offset"); if (it != info_.hardware_parameters.end()) { @@ -182,7 +183,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i auto param_it = joint.parameters.find("multiplier"); if (param_it != joint.parameters.end()) { - mimic_joint.multiplier = std::stod(joint.parameters.at("multiplier")); + mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); } mimic_joints_.push_back(mimic_joint); } @@ -375,6 +376,11 @@ return_type GenericSystem::prepare_command_mode_switch( { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; + if (!calculate_dynamics_) + { + return ret_val; + } + const size_t FOUND_ONCE_FLAG = 1000000; std::vector joint_found_in_x_requests_; @@ -705,7 +711,7 @@ void GenericSystem::initialize_storage_vectors( // Check the initial_value param is used if (!interface.initial_value.empty()) { - states[index][i] = std::stod(interface.initial_value); + states[index][i] = hardware_interface::stod(interface.initial_value); } else { @@ -713,7 +719,7 @@ void GenericSystem::initialize_storage_vectors( auto it2 = component.parameters.find("initial_" + interface.name); if (it2 != component.parameters.end()) { - states[index][i] = std::stod(it2->second); + states[index][i] = hardware_interface::stod(it2->second); print_hint = true; } else diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2d131d8d02..7f68ca88c9 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -101,6 +101,8 @@ class ResourceStorage component_info.class_type = hardware_info.hardware_class_type; hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); + hardware_used_by_controllers_.insert( + std::make_pair(component_info.name, std::vector())); } template @@ -195,6 +197,58 @@ class ResourceStorage return result; } + void remove_all_hardware_interfaces_from_available_list(const std::string & hardware_name) + { + // remove all command interfaces from available list + for (const auto & interface : hardware_info_map_[hardware_name].command_interfaces) + { + auto found_it = std::find( + available_command_interfaces_.begin(), available_command_interfaces_.end(), interface); + + if (found_it != available_command_interfaces_.end()) + { + available_command_interfaces_.erase(found_it); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "(hardware '%s'): '%s' command interface removed from available list", + hardware_name.c_str(), interface.c_str()); + } + else + { + // TODO(destogl): do here error management if interfaces are only partially added into + // "available" list - this should never be the case! + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "(hardware '%s'): '%s' command interface not in available list. " + "This should not happen (hint: multiple cleanup calls).", + hardware_name.c_str(), interface.c_str()); + } + } + // remove all state interfaces from available list + for (const auto & interface : hardware_info_map_[hardware_name].state_interfaces) + { + auto found_it = std::find( + available_state_interfaces_.begin(), available_state_interfaces_.end(), interface); + + if (found_it != available_state_interfaces_.end()) + { + available_state_interfaces_.erase(found_it); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "(hardware '%s'): '%s' state interface removed from available list", + hardware_name.c_str(), interface.c_str()); + } + else + { + // TODO(destogl): do here error management if interfaces are only partially added into + // "available" list - this should never be the case! + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "(hardware '%s'): '%s' state interface not in available list. " + "This should not happen (hint: multiple cleanup calls).", + hardware_name.c_str(), interface.c_str()); + } + } + } + template bool cleanup_hardware(HardwareT & hardware) { @@ -204,55 +258,7 @@ class ResourceStorage if (result) { - // remove all command interfaces from available list - for (const auto & interface : hardware_info_map_[hardware.get_name()].command_interfaces) - { - auto found_it = std::find( - available_command_interfaces_.begin(), available_command_interfaces_.end(), interface); - - if (found_it != available_command_interfaces_.end()) - { - available_command_interfaces_.erase(found_it); - RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", - "(hardware '%s'): '%s' command interface removed from available list", - hardware.get_name().c_str(), interface.c_str()); - } - else - { - // TODO(destogl): do here error management if interfaces are only partially added into - // "available" list - this should never be the case! - RCUTILS_LOG_WARN_NAMED( - "resource_manager", - "(hardware '%s'): '%s' command interface not in available list." - " This can happen due to multiple calls to 'cleanup'", - hardware.get_name().c_str(), interface.c_str()); - } - } - // remove all state interfaces from available list - for (const auto & interface : hardware_info_map_[hardware.get_name()].state_interfaces) - { - auto found_it = std::find( - available_state_interfaces_.begin(), available_state_interfaces_.end(), interface); - - if (found_it != available_state_interfaces_.end()) - { - available_state_interfaces_.erase(found_it); - RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", "(hardware '%s'): '%s' state interface removed from available list", - hardware.get_name().c_str(), interface.c_str()); - } - else - { - // TODO(destogl): do here error management if interfaces are only partially added into - // "available" list - this should never be the case! - RCUTILS_LOG_WARN_NAMED( - "resource_manager", - "(hardware '%s'): '%s' state interface not in available list. " - "This can happen due to multiple calls to 'cleanup'", - hardware.get_name().c_str(), interface.c_str()); - } - } + remove_all_hardware_interfaces_from_available_list(hardware.get_name()); } return result; } @@ -629,6 +635,10 @@ class ResourceStorage std::unordered_map hardware_info_map_; + /// Mapping between hardware and controllers that are using it (accessing data from it) + std::unordered_map> hardware_used_by_controllers_; + + /// Mapping between controllers and list of reference interfaces they are using std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces @@ -665,6 +675,7 @@ ResourceManager::ResourceManager( } } +// CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { is_urdf_loaded__ = true; @@ -699,6 +710,11 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac { validate_storage(hardware_info); } + + std::lock_guard guard(resources_lock_); + read_write_status.failed_hardware_names.reserve( + resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + + resource_storage_->systems_.size()); } bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } @@ -715,6 +731,7 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string & return LoanedStateInterface(resource_storage_->state_interface_map_.at(key)); } +// CM API: Called in "callback/slow"-thread std::vector ResourceManager::state_interface_keys() const { std::vector keys; @@ -726,19 +743,14 @@ std::vector ResourceManager::state_interface_keys() const return keys; } +// CM API: Called in "update"-thread std::vector ResourceManager::available_state_interfaces() const { std::lock_guard guard(resource_interfaces_lock_); return resource_storage_->available_state_interfaces_; } -bool ResourceManager::state_interface_exists(const std::string & key) const -{ - std::lock_guard guard(resource_interfaces_lock_); - return resource_storage_->state_interface_map_.find(key) != - resource_storage_->state_interface_map_.end(); -} - +// CM API: Called in "update"-thread (indirectly through `claim_state_interface`) bool ResourceManager::state_interface_is_available(const std::string & name) const { std::lock_guard guard(resource_interfaces_lock_); @@ -748,6 +760,7 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } +// CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, std::vector & interfaces) { @@ -757,12 +770,14 @@ void ResourceManager::import_controller_reference_interfaces( resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names; } +// CM API: Called in "callback/slow"-thread std::vector ResourceManager::get_controller_reference_interface_names( const std::string & controller_name) { return resource_storage_->controllers_reference_interfaces_map_.at(controller_name); } +// CM API: Called in "update"-thread void ResourceManager::make_controller_reference_interfaces_available( const std::string & controller_name) { @@ -774,6 +789,7 @@ void ResourceManager::make_controller_reference_interfaces_available( interface_names.end()); } +// CM API: Called in "update"-thread void ResourceManager::make_controller_reference_interfaces_unavailable( const std::string & controller_name) { @@ -796,6 +812,7 @@ void ResourceManager::make_controller_reference_interfaces_unavailable( } } +// CM API: Called in "callback/slow"-thread void ResourceManager::remove_controller_reference_interfaces(const std::string & controller_name) { auto interface_names = @@ -807,6 +824,53 @@ void ResourceManager::remove_controller_reference_interfaces(const std::string & resource_storage_->remove_command_interfaces(interface_names); } +// CM API: Called in "callback/slow"-thread +void ResourceManager::cache_controller_to_hardware( + const std::string & controller_name, const std::vector & interfaces) +{ + for (const auto & interface : interfaces) + { + bool found = false; + for (const auto & [hw_name, hw_info] : resource_storage_->hardware_info_map_) + { + auto cmd_itf_it = + std::find(hw_info.command_interfaces.begin(), hw_info.command_interfaces.end(), interface); + if (cmd_itf_it != hw_info.command_interfaces.end()) + { + found = true; + } + auto state_itf_it = + std::find(hw_info.state_interfaces.begin(), hw_info.state_interfaces.end(), interface); + if (state_itf_it != hw_info.state_interfaces.end()) + { + found = true; + } + + if (found) + { + // check if controller exist already in the list and if not add it + auto controllers = resource_storage_->hardware_used_by_controllers_[hw_name]; + auto ctrl_it = std::find(controllers.begin(), controllers.end(), controller_name); + if (ctrl_it == controllers.end()) + { + // add because it does not exist + controllers.reserve(controllers.size() + 1); + controllers.push_back(controller_name); + } + resource_storage_->hardware_used_by_controllers_[hw_name] = controllers; + break; + } + } + } +} + +// CM API: Called in "update"-thread +std::vector ResourceManager::get_cached_controllers_to_hardware( + const std::string & hardware_name) +{ + return resource_storage_->hardware_used_by_controllers_[hardware_name]; +} + // CM API: Called in "update"-thread bool ResourceManager::command_interface_is_claimed(const std::string & key) const { @@ -848,6 +912,7 @@ void ResourceManager::release_command_interface(const std::string & key) resource_storage_->claimed_command_interface_map_[key] = false; } +// CM API: Called in "callback/slow"-thread std::vector ResourceManager::command_interface_keys() const { std::vector keys; @@ -859,20 +924,14 @@ std::vector ResourceManager::command_interface_keys() const return keys; } +// CM API: Called in "update"-thread std::vector ResourceManager::available_command_interfaces() const { std::lock_guard guard(resource_interfaces_lock_); return resource_storage_->available_command_interfaces_; } -bool ResourceManager::command_interface_exists(const std::string & key) const -{ - std::lock_guard guard(resource_interfaces_lock_); - return resource_storage_->command_interface_map_.find(key) != - resource_storage_->command_interface_map_.end(); -} - -// CM API +// CM API: Called in "callback/slow"-thread bool ResourceManager::command_interface_is_available(const std::string & name) const { std::lock_guard guard(resource_interfaces_lock_); @@ -882,40 +941,37 @@ bool ResourceManager::command_interface_is_available(const std::string & name) c name) != resource_storage_->available_command_interfaces_.end(); } -size_t ResourceManager::actuator_components_size() const -{ - return resource_storage_->actuators_.size(); -} - -size_t ResourceManager::sensor_components_size() const -{ - return resource_storage_->sensors_.size(); -} - void ResourceManager::import_component( std::unique_ptr actuator, const HardwareInfo & hardware_info) { + std::lock_guard guard(resources_lock_); resource_storage_->initialize_actuator(std::move(actuator), hardware_info); + read_write_status.failed_hardware_names.reserve( + resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + + resource_storage_->systems_.size()); } void ResourceManager::import_component( std::unique_ptr sensor, const HardwareInfo & hardware_info) { + std::lock_guard guard(resources_lock_); resource_storage_->initialize_sensor(std::move(sensor), hardware_info); + read_write_status.failed_hardware_names.reserve( + resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + + resource_storage_->systems_.size()); } void ResourceManager::import_component( std::unique_ptr system, const HardwareInfo & hardware_info) { + std::lock_guard guard(resources_lock_); resource_storage_->initialize_system(std::move(system), hardware_info); + read_write_status.failed_hardware_names.reserve( + resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + + resource_storage_->systems_.size()); } -size_t ResourceManager::system_components_size() const -{ - return resource_storage_->systems_.size(); -} -// End of "used only in tests" - +// CM API: Called in "callback/slow"-thread std::unordered_map ResourceManager::get_components_status() { for (auto & component : resource_storage_->actuators_) @@ -934,6 +990,7 @@ std::unordered_map ResourceManager::get_comp return resource_storage_->hardware_info_map_; } +// CM API: Called in "callback/slow"-thread bool ResourceManager::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) @@ -979,6 +1036,7 @@ bool ResourceManager::prepare_command_mode_switch( return true; } +// CM API: Called in "update"-thread bool ResourceManager::perform_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) @@ -1006,6 +1064,7 @@ bool ResourceManager::perform_command_mode_switch( return true; } +// CM API: Called in "callback/slow"-thread return_type ResourceManager::set_component_state( const std::string & component_name, rclcpp_lifecycle::State & target_state) { @@ -1089,34 +1148,94 @@ return_type ResourceManager::set_component_state( return result; } -void ResourceManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) +// CM API: Called in "update"-thread +HardwareReadWriteStatus ResourceManager::read( + const rclcpp::Time & time, const rclcpp::Duration & period) { - for (auto & component : resource_storage_->actuators_) - { - component.read(time, period); - } - for (auto & component : resource_storage_->sensors_) - { - component.read(time, period); - } - for (auto & component : resource_storage_->systems_) + std::lock_guard guard(resources_lock_); + read_write_status.ok = true; + read_write_status.failed_hardware_names.clear(); + + auto read_components = [&](auto & components) { - component.read(time, period); - } + for (auto & component : components) + { + if (component.read(time, period) != return_type::OK) + { + 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()); + } + } + }; + + read_components(resource_storage_->actuators_); + read_components(resource_storage_->sensors_); + read_components(resource_storage_->systems_); + + return read_write_status; } -void ResourceManager::write(const rclcpp::Time & time, const rclcpp::Duration & period) +// CM API: Called in "update"-thread +HardwareReadWriteStatus ResourceManager::write( + const rclcpp::Time & time, const rclcpp::Duration & period) { - for (auto & component : resource_storage_->actuators_) - { - component.write(time, period); - } - for (auto & component : resource_storage_->systems_) + std::lock_guard guard(resources_lock_); + read_write_status.ok = true; + read_write_status.failed_hardware_names.clear(); + + auto write_components = [&](auto & components) { - component.write(time, period); - } + for (auto & component : components) + { + if (component.write(time, period) != return_type::OK) + { + 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()); + } + } + }; + + write_components(resource_storage_->actuators_); + write_components(resource_storage_->systems_); + + return read_write_status; +} + +// BEGIN: "used only in tests and locally" +size_t ResourceManager::actuator_components_size() const +{ + return resource_storage_->actuators_.size(); +} + +size_t ResourceManager::sensor_components_size() const +{ + return resource_storage_->sensors_.size(); +} + +size_t ResourceManager::system_components_size() const +{ + return resource_storage_->systems_.size(); +} + +bool ResourceManager::command_interface_exists(const std::string & key) const +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->command_interface_map_.find(key) != + resource_storage_->command_interface_map_.end(); } +bool ResourceManager::state_interface_exists(const std::string & key) const +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->state_interface_map_.find(key) != + resource_storage_->state_interface_map_.end(); +} +// END: "used only in tests and locally" + +// BEGIN: private methods + void ResourceManager::validate_storage( const std::vector & hardware_info) const { diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 036237221c..2e53e447b9 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -195,6 +195,12 @@ const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_st return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index a6643b3e3b..ee942d6581 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -214,6 +214,12 @@ const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_st return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -230,6 +236,12 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + if ( + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + return return_type::OK; + } return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index c656d1a692..92851303b3 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -518,6 +518,46 @@ class TestGenericSystem : public ::testing::Test )"; + valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_ = + R"( + + + mock_components/GenericSystem + true + + + + + 3.45 + + + + + + + + 2.78 + + + + + + + + + 2.78 + + + + + + + + + + +)"; + disabled_commands_ = R"( @@ -556,6 +596,7 @@ class TestGenericSystem : public ::testing::Test 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 valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_; std::string disabled_commands_; }; @@ -570,7 +611,6 @@ class TestableResourceManager : public hardware_interface::ResourceManager public: friend TestGenericSystem; - FRIEND_TEST(TestGenericSystem, generic_fake_system_2dof_symetric_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_symetric_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_asymetric_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_other_interfaces); @@ -592,8 +632,8 @@ class TestableResourceManager : public hardware_interface::ResourceManager }; void set_components_state( - hardware_interface::ResourceManager & rm, const std::vector & components, - const uint8_t state_id, const std::string & state_name) + TestableResourceManager & rm, const std::vector & components, const uint8_t state_id, + const std::string & state_name) { for (const auto & component : components) { @@ -603,7 +643,7 @@ void set_components_state( } auto configure_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -612,7 +652,7 @@ auto configure_components = []( }; auto activate_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -621,7 +661,7 @@ auto activate_components = []( }; auto deactivate_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -633,7 +673,7 @@ TEST_F(TestGenericSystem, load_generic_system_2dof) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ + ros2_control_test_assets::urdf_tail; - ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); + ASSERT_NO_THROW(TestableResourceManager rm(urdf)); } // REMOVE THIS TEST ONCE FAKE COMPONENTS ARE REMOVED @@ -641,7 +681,7 @@ TEST_F(TestGenericSystem, generic_fake_system_2dof_symetric_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_fake_system_2dof_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -672,7 +712,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -703,7 +743,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_asymetric_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -749,7 +789,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) void generic_system_functional_test(const std::string & urdf, const double offset = 0) { - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // check is hardware is configured auto status_map = rm.get_components_status(); EXPECT_EQ( @@ -859,7 +899,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_other_interface_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -942,7 +982,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1040,7 +1080,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) void TestGenericSystem::test_generic_system_with_mock_sensor_commands(const std::string & urdf) { - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1179,7 +1219,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) void TestGenericSystem::test_generic_system_with_mimic_joint(const std::string & urdf) { - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1278,7 +1318,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i const double offset = -3; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // check is hardware is configured auto status_map = rm.get_components_status(); @@ -1388,11 +1428,11 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i hardware_interface::lifecycle_state_names::INACTIVE); } -TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_) +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) { auto urdf = ros2_control_test_assets::urdf_head + valid_urdf_ros2_control_system_robot_with_gpio_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // check is hardware is started auto status_map = rm.get_components_status(); @@ -1616,7 +1656,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + sensor_with_initial_value_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1644,7 +1684,7 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + gpio_with_initial_value_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1901,3 +1941,43 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ASSERT_EQ(0.0, j1v_s.get_value()); ASSERT_EQ(0.11, j1p_c.get_value()); } + +TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tags) +{ + auto check_prepare_command_mode_switch = [&](const std::string & urdf) + { + TestableResourceManager rm( + ros2_control_test_assets::urdf_head + urdf + ros2_control_test_assets::urdf_tail); + auto start_interfaces = rm.command_interface_keys(); + std::vector stop_interfaces; + return rm.prepare_command_mode_switch(start_interfaces, stop_interfaces); + }; + + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_asymetric_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_other_interface_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_)); + ASSERT_TRUE( + check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_True_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_mimic_joint_)); + ASSERT_TRUE( + check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_with_offset_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_)); + ASSERT_TRUE(check_prepare_command_mode_switch(valid_urdf_ros2_control_system_robot_with_gpio_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_)); + ASSERT_TRUE(check_prepare_command_mode_switch(sensor_with_initial_value_)); + ASSERT_TRUE(check_prepare_command_mode_switch(gpio_with_initial_value_)); + ASSERT_FALSE(check_prepare_command_mode_switch( + hardware_system_2dof_standard_interfaces_with_different_control_modes_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_)); + ASSERT_TRUE(check_prepare_command_mode_switch(disabled_commands_)); +} diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 128771058b..d90756c324 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -446,17 +446,17 @@ TEST(TestComponentInterfaces, dummy_actuator) double velocity_value = 1.0; command_interfaces[0].set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } state = actuator_hw.configure(); @@ -496,12 +496,12 @@ TEST(TestComponentInterfaces, dummy_actuator) // Noting should change because it is FINALIZED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value EXPECT_EQ(0, state_interfaces[1].get_value()); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } EXPECT_EQ( @@ -587,12 +587,12 @@ TEST(TestComponentInterfaces, dummy_system) command_interfaces[0].set_value(velocity_value); // velocity command_interfaces[1].set_value(velocity_value); // velocity command_interfaces[2].set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity @@ -601,7 +601,7 @@ TEST(TestComponentInterfaces, dummy_system) ASSERT_TRUE(std::isnan(state_interfaces[4].get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[5].get_value())); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } state = system_hw.configure(); @@ -649,7 +649,7 @@ TEST(TestComponentInterfaces, dummy_system) // Noting should change because it is FINALIZED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value EXPECT_EQ(0.0, state_interfaces[1].get_value()); // velocity @@ -658,7 +658,7 @@ TEST(TestComponentInterfaces, dummy_system) EXPECT_EQ(20 * velocity_value, state_interfaces[4].get_value()); // position value EXPECT_EQ(0.0, state_interfaces[5].get_value()); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); @@ -841,6 +841,12 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + // Noting should change because it is UNCONFIGURED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + // activate again and expect reset values state = sensor_hw.configure(); EXPECT_EQ(state_interfaces[0].get_value(), 0.0); @@ -860,6 +866,12 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + // Noting should change because it is FINALIZED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + // can not change state anymore state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index b19c10169e..cdd2f2bb17 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -500,6 +500,30 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) EXPECT_EQ(transmission.parameters.at("additional_special_parameter"), "1337"); } +TEST_F(TestComponentParser, successfully_parse_locale_independent_double) +{ + // Set to locale with comma-separated decimals + std::setlocale(LC_NUMERIC, "de_DE.UTF-8"); + + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_actuator_only + + ros2_control_test_assets::urdf_tail; + + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + const auto hardware_info = control_hardware.at(0); + + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.13"); + + ASSERT_THAT(hardware_info.transmissions, SizeIs(1)); + const auto transmission = hardware_info.transmissions[0]; + EXPECT_THAT(transmission.joints, SizeIs(1)); + const auto joint = transmission.joints[0]; + + // Test that we still parse doubles using dot notation + EXPECT_THAT(joint.mechanical_reduction, DoubleEq(325.949)); +} + TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio) { std::string urdf_to_test = diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 47dd9f0d32..cdbdbd930a 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -75,6 +75,13 @@ 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) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_ = 0.0; + return return_type::ERROR; + } // 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 @@ -85,6 +92,13 @@ 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) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_ = 0.0; + return return_type::ERROR; + } return return_type::OK; } @@ -95,5 +109,15 @@ class TestActuator : public ActuatorInterface double max_velocity_command_ = 0.0; }; +class TestUnitilizableActuator : public TestActuator +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + ActuatorInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuator, hardware_interface::ActuatorInterface) diff --git a/hardware_interface/test/test_components/test_components.xml b/hardware_interface/test/test_components/test_components.xml index 235fec668f..f029d3dd37 100644 --- a/hardware_interface/test/test_components/test_components.xml +++ b/hardware_interface/test/test_components/test_components.xml @@ -17,4 +17,22 @@ Test System + + + + Test Unitilizable Actuator + + + + + + Test Unitilizable Sensor + + + + + + Test Unitilizable System + + diff --git a/hardware_interface/test/test_components/test_sensor.cpp b/hardware_interface/test/test_components/test_sensor.cpp index ae31c73436..4811244138 100644 --- a/hardware_interface/test/test_components/test_sensor.cpp +++ b/hardware_interface/test/test_components/test_sensor.cpp @@ -55,5 +55,15 @@ class TestSensor : public SensorInterface double velocity_state_ = 0.0; }; +class TestUnitilizableSensor : public TestSensor +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + SensorInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableSensor, hardware_interface::SensorInterface) diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index f198e057da..9792286796 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -83,11 +83,25 @@ 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) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_[0] = 0.0; + return return_type::ERROR; + } 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) + { + // reset value to get out from error on the next call - simplifies CM tests + velocity_command_[0] = 0.0; + return return_type::ERROR; + } return return_type::OK; } @@ -101,5 +115,15 @@ class TestSystem : public SystemInterface double configuration_command_ = 0.0; }; +class TestUnitilizableSystem : public TestSystem +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + SystemInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableSystem, hardware_interface::SystemInterface) diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 12cc597dc7..e799a47fcd 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -68,6 +68,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); + FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation); TestableResourceManager() : hardware_interface::ResourceManager() {} @@ -98,7 +99,7 @@ std::vector set_components_state( } auto configure_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, @@ -106,7 +107,7 @@ auto configure_components = }; auto activate_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, @@ -114,7 +115,7 @@ auto activate_components = }; auto deactivate_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, @@ -122,7 +123,7 @@ auto deactivate_components = }; auto cleanup_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, @@ -130,7 +131,7 @@ auto cleanup_components = }; auto shutdown_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, @@ -139,25 +140,63 @@ auto shutdown_components = TEST_F(ResourceManagerTest, initialization_empty) { - ASSERT_ANY_THROW(hardware_interface::ResourceManager rm("")); + ASSERT_ANY_THROW(TestableResourceManager rm("")); } TEST_F(ResourceManagerTest, initialization_with_urdf) { - ASSERT_NO_THROW( - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf)); + ASSERT_NO_THROW(TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf)); } TEST_F(ResourceManagerTest, post_initialization_with_urdf) { - hardware_interface::ResourceManager rm; + TestableResourceManager rm; ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); } +TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) +{ + // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a + // runtime exception is thrown + TestableResourceManager rm; + ASSERT_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true), + std::runtime_error); +} + +TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) +{ + // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, + // the interface should not show up + TestableResourceManager rm; + EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + + // test actuator + EXPECT_FALSE(rm.state_interface_exists("joint1/position")); + EXPECT_FALSE(rm.state_interface_exists("joint1/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint1/position")); + EXPECT_FALSE(rm.command_interface_exists("joint1/max_velocity")); + + // test sensor + EXPECT_FALSE(rm.state_interface_exists("sensor1/velocity")); + + // test system + EXPECT_FALSE(rm.state_interface_exists("joint2/position")); + EXPECT_FALSE(rm.state_interface_exists("joint2/velocity")); + EXPECT_FALSE(rm.state_interface_exists("joint2/acceleration")); + EXPECT_FALSE(rm.command_interface_exists("joint2/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint2/max_acceleration")); + EXPECT_FALSE(rm.state_interface_exists("joint3/position")); + EXPECT_FALSE(rm.state_interface_exists("joint3/velocity")); + EXPECT_FALSE(rm.state_interface_exists("joint3/acceleration")); + EXPECT_FALSE(rm.command_interface_exists("joint3/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration")); +} + TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) { // we validate the results manually - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); EXPECT_EQ(1u, rm.actuator_components_size()); EXPECT_EQ(1u, rm.sensor_components_size()); @@ -178,24 +217,18 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } -TEST_F(ResourceManagerTest, when_missing_state_keys_expect_hw_initialization_fails) +TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) { // missing state keys { EXPECT_THROW( - hardware_interface::ResourceManager rm( - ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); } -} - -TEST_F(ResourceManagerTest, when_missing_command_keys_expect_hw_initialization_fails) -{ // missing command keys { EXPECT_THROW( - hardware_interface::ResourceManager rm( - ros2_control_test_assets::minimal_robot_missing_command_keys_urdf), + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_missing_command_keys_urdf), std::exception); } } @@ -203,7 +236,7 @@ TEST_F(ResourceManagerTest, when_missing_command_keys_expect_hw_initialization_f TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) { // we validate the results manually - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); auto command_interface_keys = rm.command_interface_keys(); for (const auto & key : command_interface_keys) @@ -249,7 +282,7 @@ TEST_F(ResourceManagerTest, can_load_urdf_later) TEST_F(ResourceManagerTest, resource_claiming) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available activate_components(rm); @@ -361,7 +394,7 @@ class ExternalComponent : public hardware_interface::ActuatorInterface TEST_F(ResourceManagerTest, post_initialization_add_components) { // we validate the results manually - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); // Activate components to get all interfaces available activate_components(rm); @@ -404,7 +437,7 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) TEST_F(ResourceManagerTest, default_prepare_perform_switch) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available activate_components(rm); @@ -438,7 +471,7 @@ const auto command_mode_urdf = std::string(ros2_control_test_assets::urdf_head) TEST_F(ResourceManagerTest, custom_prepare_perform_switch) { - hardware_interface::ResourceManager rm(command_mode_urdf); + TestableResourceManager rm(command_mode_urdf); // Scenarios defined by example criteria std::vector empty_keys = {}; std::vector irrelevant_keys = {"elbow_joint/position", "should_joint/position"}; @@ -474,7 +507,7 @@ TEST_F(ResourceManagerTest, custom_prepare_perform_switch) TEST_F(ResourceManagerTest, resource_status) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); auto status_map = rm.get_components_status(); @@ -546,7 +579,7 @@ TEST_F(ResourceManagerTest, resource_status) TEST_F(ResourceManagerTest, lifecycle_all_resources) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // All resources start as UNCONFIGURED { @@ -689,7 +722,7 @@ TEST_F(ResourceManagerTest, lifecycle_all_resources) TEST_F(ResourceManagerTest, lifecycle_individual_resources) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // All resources start as UNCONFIGURED { @@ -902,7 +935,7 @@ TEST_F(ResourceManagerTest, lifecycle_individual_resources) TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { using std::placeholders::_1; - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); auto check_interfaces = [](const std::vector & interface_names, auto check_method, bool expected_result) @@ -946,50 +979,44 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) check_interfaces( command_interface_names, - std::bind(&hardware_interface::ResourceManager::command_interface_is_claimed, &rm, _1), - expected_result); + std::bind(&TestableResourceManager::command_interface_is_claimed, &rm, _1), expected_result); }; // All resources start as UNCONFIGURED - All interfaces are imported but not available { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), false); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), false); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); } // Nothing can be claimed @@ -1006,28 +1033,23 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( {"joint1/position"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( {"joint1/max_velocity"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), false); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); } // Can claim Actuator's interfaces @@ -1045,24 +1067,20 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), false); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); } // Can claim all Actuator's state interfaces and command interfaces @@ -1078,20 +1096,20 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); } // When Sensor and System are configured their state- @@ -1100,26 +1118,23 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( {"joint2/velocity", "joint3/velocity"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( {"joint2/max_acceleration", "configuration/max_tcp_jerk"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); } // Can claim: @@ -1141,22 +1156,20 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); } // Can claim everything @@ -1176,26 +1189,23 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( {"joint1/position"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( {"joint1/max_velocity"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); } // Can claim everything @@ -1216,27 +1226,23 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( {"joint1/position"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( {"joint1/max_velocity"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); } // Can claim everything @@ -1256,26 +1262,26 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); } } TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); std::string CONTROLLER_NAME = "test_controller"; std::vector REFERENCE_INTERFACE_NAMES = {"input1", "input2", "input3"}; @@ -1388,3 +1394,265 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) EXPECT_THROW( rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range); } + +class ResourceManagerTestReadWriteError : public ResourceManagerTest + +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + rm = std::make_shared( + + ros2_control_test_assets::minimal_robot_urdf, false); + activate_components(*rm); + + 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_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + using FunctionT = + std::function; + + void check_read_or_write_failure( + FunctionT method_that_fails, FunctionT other_method, const double fail_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); + + // read failure for TEST_ACTUATOR_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value); + claimed_itfs[1].set_value(fail_value - 10.0); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, + testing::ElementsAreArray(std::vector({TEST_ACTUATOR_HARDWARE_NAME}))); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(false, true); + 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); + } + + // read failure for TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value - 10.0); + claimed_itfs[1].set_value(fail_value); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, + testing::ElementsAreArray(std::vector({TEST_SYSTEM_HARDWARE_NAME}))); + 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_UNCONFIGURED); + check_if_interface_available(true, false); + 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); + } + + // read failure for both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value); + claimed_itfs[1].set_value(fail_value); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, testing::ElementsAreArray(std::vector( + {TEST_ACTUATOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME}))); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + check_if_interface_available(false, false); + 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; + + const rclcpp::Time time = rclcpp::Time(0); + 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) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // 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); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // 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); +} + +TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) +{ + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + activate_components(rm); + + static const std::string TEST_CONTROLLER_ACTUATOR_NAME = "test_controller_actuator"; + static const std::string TEST_CONTROLLER_SYSTEM_NAME = "test_controller_system"; + static const std::string TEST_BROADCASTER_ALL_NAME = "test_broadcaster_all"; + static const std::string TEST_BROADCASTER_SENSOR_NAME = "test_broadcaster_sensor"; + + rm.cache_controller_to_hardware( + TEST_CONTROLLER_ACTUATOR_NAME, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES); + rm.cache_controller_to_hardware( + TEST_BROADCASTER_ALL_NAME, TEST_ACTUATOR_HARDWARE_STATE_INTERFACES); + + rm.cache_controller_to_hardware( + TEST_CONTROLLER_SYSTEM_NAME, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES); + rm.cache_controller_to_hardware(TEST_BROADCASTER_ALL_NAME, TEST_SYSTEM_HARDWARE_STATE_INTERFACES); + + rm.cache_controller_to_hardware( + TEST_BROADCASTER_SENSOR_NAME, TEST_SENSOR_HARDWARE_STATE_INTERFACES); + rm.cache_controller_to_hardware(TEST_BROADCASTER_ALL_NAME, TEST_SENSOR_HARDWARE_STATE_INTERFACES); + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_ACTUATOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_SYSTEM_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_SENSOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_BROADCASTER_SENSOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } +} diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index d48cd3736d..7ba8e63229 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.36.1 (2024-01-08) +------------------- + +2.36.0 (2023-12-12) +------------------- + +2.35.1 (2023-11-27) +------------------- + 2.35.0 (2023-11-14) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index fc465a6611..47b9a1b081 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 2.35.0 + 2.36.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 a03df887fb..4bd184e6b5 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.36.1 (2024-01-08) +------------------- + +2.36.0 (2023-12-12) +------------------- + +2.35.1 (2023-11-27) +------------------- + 2.35.0 (2023-11-14) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index a03cae7fde..fd34fde3a8 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 2.35.0 + 2.36.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 dbfd24b210..b35dae3cc3 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.36.1 (2024-01-08) +------------------- +* [ResourceManager] adds test for uninitialized hardware (`#1243 `_) (`#1274 `_) +* Contributors: mergify[bot] + +2.36.0 (2023-12-12) +------------------- + +2.35.1 (2023-11-27) +------------------- + 2.35.0 (2023-11-14) ------------------- diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 83281bd3d7..0525953091 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -176,6 +176,55 @@ const auto hardware_resources = )"; +const auto unitilizable_hardware_resources = + R"( + + + test_unitilizable_actuator + + + + + + + + + + + test_unitilizable_sensor + 2 + 2 + + + + + + + + test_unitilizable_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + const auto hardware_resources_missing_state_keys = R"( @@ -406,6 +455,8 @@ const auto diffbot_urdf = const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); +const auto minimal_unitilizable_robot_urdf = + std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail); const auto minimal_robot_missing_state_keys_urdf = std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) + diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 6b0f4462bc..b3cad63f01 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 2.35.0 + 2.36.1 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 8cca82acf3..41bac9c48b 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.36.1 (2024-01-08) +------------------- +* [docs] Remove joint_state_controller (`#1263 `_) (`#1264 `_) +* Contributors: mergify[bot] + +2.36.0 (2023-12-12) +------------------- + +2.35.1 (2023-11-27) +------------------- + 2.35.0 (2023-11-14) ------------------- diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index bb21b7f005..ce16d237c5 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -73,7 +73,7 @@ Example output: $ ros2 control list_controller_types diff_drive_controller/DiffDriveController controller_interface::ControllerInterface - joint_state_controller/JointStateController controller_interface::ControllerInterface + joint_state_broadcaster/JointStateBroadcaster controller_interface::ControllerInterface joint_trajectory_controller/JointTrajectoryController controller_interface::ControllerInterface diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 95408d7fd5..73358a1100 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 2.35.0 + 2.36.1 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 0eb78350f0..668c529dc5 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version='2.35.0', + version='2.36.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 fd39f8773f..e4a9321c85 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.36.1 (2024-01-08) +------------------- + +2.36.0 (2023-12-12) +------------------- + +2.35.1 (2023-11-27) +------------------- + 2.35.0 (2023-11-14) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 2df062a1c7..e80107704d 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 2.35.0 + 2.36.1 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index cbd6227409..54a3dc5c77 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -168,17 +168,22 @@ def _list_controllers(self): @rtype [str] """ # Add loaded controllers first - controllers = list_controllers(self._node, self._cm_name).controller - - # Append potential controller configs found in the node's parameters - for name in _get_parameter_controller_names(self._node, self._cm_name): - add_ctrl = all(name != ctrl.name for ctrl in controllers) - if add_ctrl: - type_str = _get_controller_type(self._node, self._cm_name, name) - uninit_ctrl = ControllerState(name=name, - type=type_str) - controllers.append(uninit_ctrl) - return controllers + try: + controllers = list_controllers( + self._node, self._cm_name, 2.0 / self._cm_update_freq + ).controller + + # Append potential controller configs found in the node's parameters + for name in _get_parameter_controller_names(self._node, self._cm_name): + add_ctrl = all(name != ctrl.name for ctrl in controllers) + if add_ctrl: + type_str = _get_controller_type(self._node, self._cm_name, name) + uninit_ctrl = ControllerState(name=name, type=type_str) + controllers.append(uninit_ctrl) + return controllers + except RuntimeError as e: + print(e) + return [] def _show_controllers(self): table_view = self._widget.table_view diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index b087f6f057..2f1d13a0f0 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version='2.35.0', + version='2.36.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 de71424b81..b1e46a8a4d 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.36.1 (2024-01-08) +------------------- +* Improve transmission tests (`#1238 `_) (`#1241 `_) +* Contributors: mergify[bot] + +2.36.0 (2023-12-12) +------------------- + +2.35.1 (2023-11-27) +------------------- + 2.35.0 (2023-11-14) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index a6662edb1e..ff5e93f3d6 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 2.35.0 + 2.36.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 diff --git a/transmission_interface/test/differential_transmission_loader_test.cpp b/transmission_interface/test/differential_transmission_loader_test.cpp index d2ed533652..70d0adf2cd 100644 --- a/transmission_interface/test/differential_transmission_loader_test.cpp +++ b/transmission_interface/test/differential_transmission_loader_test.cpp @@ -26,8 +26,12 @@ #include "transmission_interface/differential_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" +using testing::DoubleNear; using testing::SizeIs; +// Floating-point value comparison threshold +const double EPS = 1e-5; + class TransmissionPluginLoader { public: @@ -113,16 +117,16 @@ TEST(DifferentialTransmissionLoaderTest, FullSpec) const std::vector & actuator_reduction = differential_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = differential_transmission->get_joint_reduction(); - EXPECT_EQ(2.0, joint_reduction[0]); - EXPECT_EQ(-2.0, joint_reduction[1]); + EXPECT_THAT(2.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = differential_transmission->get_joint_offset(); - EXPECT_EQ(0.5, joint_offset[0]); - EXPECT_EQ(-0.5, joint_offset[1]); + EXPECT_THAT(0.5, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(-0.5, DoubleNear(joint_offset[1], EPS)); } TEST(DifferentialTransmissionLoaderTest, only_mech_red_specified) @@ -184,16 +188,16 @@ TEST(DifferentialTransmissionLoaderTest, only_mech_red_specified) const std::vector & actuator_reduction = differential_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = differential_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = differential_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) @@ -247,16 +251,16 @@ TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) const std::vector & actuator_reduction = differential_transmission->get_actuator_reduction(); - EXPECT_EQ(1.0, actuator_reduction[0]); - EXPECT_EQ(1.0, actuator_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = differential_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = differential_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(DifferentialTransmissionLoaderTest, mechanical_reduction_not_a_number) @@ -319,16 +323,16 @@ TEST(DifferentialTransmissionLoaderTest, mechanical_reduction_not_a_number) // default kicks in for ill-defined values const std::vector & actuator_reduction = differential_transmission->get_actuator_reduction(); - EXPECT_EQ(1.0, actuator_reduction[0]); - EXPECT_EQ(1.0, actuator_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = differential_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = differential_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(DifferentialTransmissionLoaderTest, offset_ill_defined) @@ -393,17 +397,17 @@ TEST(DifferentialTransmissionLoaderTest, offset_ill_defined) // default kicks in for ill-defined values const std::vector & actuator_reduction = differential_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = differential_transmission->get_joint_reduction(); - EXPECT_EQ(2.0, joint_reduction[0]); - EXPECT_EQ(-2.0, joint_reduction[1]); + EXPECT_THAT(2.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(joint_reduction[1], EPS)); // default kicks in for ill-defined values const std::vector & joint_offset = differential_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(DifferentialTransmissionLoaderTest, mech_red_invalid_value) diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index 7e27194bb6..14ffe968bc 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -29,7 +29,7 @@ using transmission_interface::DifferentialTransmission; using transmission_interface::Exception; using transmission_interface::JointHandle; // Floating-point value comparison threshold -const double EPS = 1e-6; +const double EPS = 1e-5; TEST(PreconditionsTest, ExceptionThrowing) { @@ -79,12 +79,12 @@ TEST(PreconditionsTest, AccessorValidation) EXPECT_EQ(2u, trans.num_actuators()); EXPECT_EQ(2u, trans.num_joints()); - EXPECT_EQ(2.0, trans.get_actuator_reduction()[0]); - EXPECT_EQ(-2.0, trans.get_actuator_reduction()[1]); - EXPECT_EQ(4.0, trans.get_joint_reduction()[0]); - EXPECT_EQ(-4.0, trans.get_joint_reduction()[1]); - EXPECT_EQ(1.0, trans.get_joint_offset()[0]); - EXPECT_EQ(-1.0, trans.get_joint_offset()[1]); + EXPECT_THAT(2.0, DoubleNear(trans.get_actuator_reduction()[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(trans.get_actuator_reduction()[1], EPS)); + EXPECT_THAT(4.0, DoubleNear(trans.get_joint_reduction()[0], EPS)); + EXPECT_THAT(-4.0, DoubleNear(trans.get_joint_reduction()[1], EPS)); + EXPECT_THAT(1.0, DoubleNear(trans.get_joint_offset()[0], EPS)); + EXPECT_THAT(-1.0, DoubleNear(trans.get_joint_offset()[1], EPS)); } void testConfigureWithBadHandles(std::string interface_name) diff --git a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp index d5960ab47c..53b584b7b5 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_loader_test.cpp @@ -26,8 +26,12 @@ #include "transmission_interface/four_bar_linkage_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" +using testing::DoubleNear; using testing::SizeIs; +// Floating-point value comparison threshold +const double EPS = 1e-5; + class TransmissionPluginLoader { public: @@ -113,17 +117,17 @@ TEST(FourBarLinkageTransmissionLoaderTest, FullSpec) const std::vector & actuator_reduction = four_bar_linkage_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = four_bar_linkage_transmission->get_joint_reduction(); - EXPECT_EQ(2.0, joint_reduction[0]); - EXPECT_EQ(-2.0, joint_reduction[1]); + EXPECT_THAT(2.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); - EXPECT_EQ(0.5, joint_offset[0]); - EXPECT_EQ(-0.5, joint_offset[1]); + EXPECT_THAT(0.5, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(-0.5, DoubleNear(joint_offset[1], EPS)); } TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) @@ -185,17 +189,17 @@ TEST(FourBarLinkageTransmissionLoaderTest, only_mech_red_specified) const std::vector & actuator_reduction = four_bar_linkage_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = four_bar_linkage_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) @@ -249,17 +253,17 @@ TEST(DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified) const std::vector & actuator_reduction = four_bar_linkage_transmission->get_actuator_reduction(); - EXPECT_EQ(1.0, actuator_reduction[0]); - EXPECT_EQ(1.0, actuator_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = four_bar_linkage_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) @@ -322,17 +326,17 @@ TEST(FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number) // default kicks in for ill-defined values const std::vector & actuator_reduction = four_bar_linkage_transmission->get_actuator_reduction(); - EXPECT_EQ(1.0, actuator_reduction[0]); - EXPECT_EQ(1.0, actuator_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = four_bar_linkage_transmission->get_joint_reduction(); - EXPECT_EQ(1.0, joint_reduction[0]); - EXPECT_EQ(1.0, joint_reduction[1]); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(1.0, DoubleNear(joint_reduction[1], EPS)); const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) @@ -397,18 +401,18 @@ TEST(FourBarLinkageTransmissionLoaderTest, offset_ill_defined) // default kicks in for ill-defined values const std::vector & actuator_reduction = four_bar_linkage_transmission->get_actuator_reduction(); - EXPECT_EQ(50.0, actuator_reduction[0]); - EXPECT_EQ(-50.0, actuator_reduction[1]); + EXPECT_THAT(50.0, DoubleNear(actuator_reduction[0], EPS)); + EXPECT_THAT(-50.0, DoubleNear(actuator_reduction[1], EPS)); const std::vector & joint_reduction = four_bar_linkage_transmission->get_joint_reduction(); - EXPECT_EQ(2.0, joint_reduction[0]); - EXPECT_EQ(-2.0, joint_reduction[1]); + EXPECT_THAT(2.0, DoubleNear(joint_reduction[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(joint_reduction[1], EPS)); // default kicks in for ill-defined values const std::vector & joint_offset = four_bar_linkage_transmission->get_joint_offset(); - EXPECT_EQ(0.0, joint_offset[0]); - EXPECT_EQ(0.0, joint_offset[1]); + EXPECT_THAT(0.0, DoubleNear(joint_offset[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint_offset[1], EPS)); } TEST(FourBarLinkageTransmissionLoaderTest, mech_red_invalid_value) diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index a44feb178c..f74e4def6a 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_test.cpp @@ -33,7 +33,7 @@ using transmission_interface::FourBarLinkageTransmission; using transmission_interface::JointHandle; // Floating-point value comparison threshold -const double EPS = 1e-6; +const double EPS = 1e-5; TEST(PreconditionsTest, ExceptionThrowing) { @@ -83,12 +83,12 @@ TEST(PreconditionsTest, AccessorValidation) EXPECT_EQ(2u, trans.num_actuators()); EXPECT_EQ(2u, trans.num_joints()); - EXPECT_EQ(2.0, trans.get_actuator_reduction()[0]); - EXPECT_EQ(-2.0, trans.get_actuator_reduction()[1]); - EXPECT_EQ(4.0, trans.get_joint_reduction()[0]); - EXPECT_EQ(-4.0, trans.get_joint_reduction()[1]); - EXPECT_EQ(1.0, trans.get_joint_offset()[0]); - EXPECT_EQ(-1.0, trans.get_joint_offset()[1]); + EXPECT_THAT(2.0, DoubleNear(trans.get_actuator_reduction()[0], EPS)); + EXPECT_THAT(-2.0, DoubleNear(trans.get_actuator_reduction()[1], EPS)); + EXPECT_THAT(4.0, DoubleNear(trans.get_joint_reduction()[0], EPS)); + EXPECT_THAT(-4.0, DoubleNear(trans.get_joint_reduction()[1], EPS)); + EXPECT_THAT(1.0, DoubleNear(trans.get_joint_offset()[0], EPS)); + EXPECT_THAT(-1.0, DoubleNear(trans.get_joint_offset()[1], EPS)); } void testConfigureWithBadHandles(std::string interface_name) diff --git a/transmission_interface/test/simple_transmission_loader_test.cpp b/transmission_interface/test/simple_transmission_loader_test.cpp index 1518bacc3b..4623d8c409 100644 --- a/transmission_interface/test/simple_transmission_loader_test.cpp +++ b/transmission_interface/test/simple_transmission_loader_test.cpp @@ -26,8 +26,12 @@ #include "transmission_interface/simple_transmission_loader.hpp" #include "transmission_interface/transmission_loader.hpp" +using testing::DoubleNear; using testing::SizeIs; +// Floating-point value comparison threshold +const double EPS = 1e-5; + class TransmissionPluginLoader { public: @@ -229,8 +233,8 @@ TEST(SimpleTransmissionLoaderTest, FullSpec) transmission_interface::SimpleTransmission * simple_transmission = dynamic_cast(transmission.get()); ASSERT_TRUE(nullptr != simple_transmission); - EXPECT_EQ(325.949, simple_transmission->get_actuator_reduction()); - EXPECT_EQ(0.0, simple_transmission->get_joint_offset()); + EXPECT_THAT(325.949, DoubleNear(simple_transmission->get_actuator_reduction(), EPS)); + EXPECT_THAT(0.0, DoubleNear(simple_transmission->get_joint_offset(), EPS)); } TEST(SimpleTransmissionLoaderTest, only_mech_red_specified) @@ -275,8 +279,8 @@ TEST(SimpleTransmissionLoaderTest, only_mech_red_specified) transmission_interface::SimpleTransmission * simple_transmission = dynamic_cast(transmission.get()); ASSERT_TRUE(nullptr != simple_transmission); - EXPECT_EQ(50.0, simple_transmission->get_actuator_reduction()); - EXPECT_EQ(0.0, simple_transmission->get_joint_offset()); + EXPECT_THAT(50.0, DoubleNear(simple_transmission->get_actuator_reduction(), EPS)); + EXPECT_THAT(0.0, DoubleNear(simple_transmission->get_joint_offset(), EPS)); } TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) @@ -317,8 +321,8 @@ TEST(SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified) transmission_interface::SimpleTransmission * simple_transmission = dynamic_cast(transmission.get()); ASSERT_TRUE(nullptr != simple_transmission); - EXPECT_EQ(1.0, simple_transmission->get_actuator_reduction()); - EXPECT_EQ(0.0, simple_transmission->get_joint_offset()); + EXPECT_THAT(1.0, DoubleNear(simple_transmission->get_actuator_reduction(), EPS)); + EXPECT_THAT(0.0, DoubleNear(simple_transmission->get_joint_offset(), EPS)); } TEST(SimpleTransmissionLoaderTest, mechanical_reduction_not_a_number) @@ -360,7 +364,7 @@ TEST(SimpleTransmissionLoaderTest, mechanical_reduction_not_a_number) dynamic_cast(transmission.get()); ASSERT_TRUE(nullptr != simple_transmission); // default kicks in for ill-defined values - EXPECT_EQ(1.0, simple_transmission->get_actuator_reduction()); + EXPECT_THAT(1.0, DoubleNear(simple_transmission->get_actuator_reduction(), EPS)); } TEST(SimpleTransmissionLoaderTest, offset_ill_defined) @@ -403,8 +407,8 @@ TEST(SimpleTransmissionLoaderTest, offset_ill_defined) dynamic_cast(transmission.get()); ASSERT_TRUE(nullptr != simple_transmission); // default kicks in for ill-defined values - EXPECT_EQ(0.0, simple_transmission->get_joint_offset()); - EXPECT_EQ(50.0, simple_transmission->get_actuator_reduction()); + EXPECT_THAT(0.0, DoubleNear(simple_transmission->get_joint_offset(), EPS)); + EXPECT_THAT(50.0, DoubleNear(simple_transmission->get_actuator_reduction(), EPS)); } TEST(SimpleTransmissionLoaderTest, mech_red_invalid_value) diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index d7601cdd20..33a14f92d1 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -27,8 +27,10 @@ using transmission_interface::Exception; using transmission_interface::JointHandle; using transmission_interface::SimpleTransmission; +using testing::DoubleNear; + // Floating-point value comparison threshold -const double EPS = 1e-6; +const double EPS = 1e-5; TEST(PreconditionsTest, ExceptionThrownWithInvalidParameters) { @@ -53,8 +55,8 @@ TEST(PreconditionsTest, AccessorValidation) EXPECT_EQ(1u, trans.num_actuators()); EXPECT_EQ(1u, trans.num_joints()); - EXPECT_EQ(2.0, trans.get_actuator_reduction()); - EXPECT_EQ(-1.0, trans.get_joint_offset()); + EXPECT_THAT(2.0, DoubleNear(trans.get_actuator_reduction(), EPS)); + EXPECT_THAT(-1.0, DoubleNear(trans.get_joint_offset(), EPS)); } TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) @@ -127,7 +129,7 @@ class BlackBoxTest : public TransmissionSetup, public ::testing::TestWithParam