diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index a2d02ed8a0..b3d0917732 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,10 +16,10 @@ 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: actions/checkout@v4 - uses: ros-tooling/action-ros-ci@0.2.6 with: target-ros2-distro: ${{ env.ROS_DISTRO }} diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 69f071c83d..a39647ec0b 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: name: Format runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: actions/setup-python@v2 with: python-version: '3.10' diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 009c394bfe..4d6c45c53d 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -11,8 +11,8 @@ jobs: matrix: linter: [cppcheck, copyright, lint_cmake] 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 @@ -35,8 +35,8 @@ jobs: 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 diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 3ea9a63541..ebb87d003f 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -22,10 +22,10 @@ jobs: uses: docker/setup-buildx-action@v1 - 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: | @@ -62,10 +62,10 @@ jobs: uses: docker/setup-buildx-action@v1 - 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: | 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..5d6c5af83b 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,10 +26,10 @@ 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 diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index f7ae929e5b..f0e49aac9c 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -7,7 +7,7 @@ jobs: test: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 - uses: uesteibar/reviewer-lottery@v2 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 ab6d2c3c45..7459759d6b 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.35.1 (2023-11-27) +------------------- + +2.35.0 (2023-11-14) +------------------- + +2.34.0 (2023-11-08) +------------------- + 2.33.0 (2023-10-11) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 8751107192..0a47475b74 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 2.33.0 + 2.35.1 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 770b81b341..b1f8e09223 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 `_) +* [CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (backport `#940 `_) (`#1052 `_) +* Contributors: Sai Kishor Kothakota, Denis Stogl + +2.34.0 (2023-11-08) +------------------- +* [Humble] Controller sorting (`#1157 `_) +* Update spawner to accept controllers list and start them in sequence (backport `#1139 `_) (`#1149 `_) +* Create doc file for chained controllers (backport `#985 `_) (`#1131 `_) +* Contributors: Sai Kishor Kothakota, mergify[bot] + 2.33.0 (2023-10-11) ------------------- * Export of the get_cm_node_options() from robostack (`#1129 `_) (`#1130 `_) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index fe21e37027..dce3b1bdc9 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp realtime_tools + std_msgs ) find_package(ament_cmake REQUIRED) @@ -141,6 +142,17 @@ if(BUILD_TESTING) controller_manager_msgs ros2_control_test_assets ) + ament_add_gmock(test_controller_manager_urdf_passing + test/test_controller_manager_urdf_passing.cpp + ) + target_include_directories(test_controller_manager_urdf_passing PRIVATE include) + target_link_libraries(test_controller_manager_urdf_passing + controller_manager + ) + ament_target_dependencies(test_controller_manager_urdf_passing + controller_manager_msgs + ros2_control_test_assets + ) add_library(test_controller_with_interfaces SHARED test/test_controller_with_interfaces/test_controller_with_interfaces.cpp) target_include_directories(test_controller_with_interfaces PRIVATE include) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 250357c6c8..27823c8cfd 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,6 +19,8 @@ import sys import time import warnings +import io +from contextlib import redirect_stdout, redirect_stderr from controller_manager import configure_controller, list_controllers, \ load_controller, switch_controllers, unload_controller @@ -120,8 +122,7 @@ def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() - parser.add_argument( - 'controller_name', help='Name of the controller') + parser.add_argument("controller_names", help="List of controllers", nargs="+") parser.add_argument( '-c', '--controller-manager', help='Name of the controller manager ROS node', default='controller_manager', required=False) @@ -153,10 +154,17 @@ def main(args=None): parser.add_argument( '--controller-manager-timeout', help='Time to wait for the controller manager', required=False, default=10, type=int) + parser.add_argument( + "--activate-as-group", + help="Activates all the parsed controllers list together instead of one by one." + " Useful for activating all chainable controllers altogether", + 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_name = args.controller_name + controller_names = args.controller_names controller_manager_name = args.controller_manager controller_namespace = args.namespace param_file = args.param_file @@ -166,12 +174,9 @@ def main(args=None): if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) - prefixed_controller_name = controller_name - if controller_namespace: - prefixed_controller_name = controller_namespace + '/' + controller_name + node = Node("spawner_" + controller_names[0]) - node = Node('spawner_' + controller_name) - if not controller_manager_name.startswith('/'): + if not controller_manager_name.startswith("/"): spawner_namespace = node.get_namespace() if spawner_namespace != '/': controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" @@ -184,64 +189,144 @@ def main(args=None): node.get_logger().error('Controller manager not available') return 1 - if is_controller_loaded(node, controller_manager_name, prefixed_controller_name): - node.get_logger().warn('Controller already loaded, skipping load_controller') - else: - if controller_type: - parameter = Parameter() - parameter.name = prefixed_controller_name + '.type' - parameter.value = get_parameter_value(string_value=controller_type) - - response = call_set_parameters( - node=node, node_name=controller_manager_name, parameters=[parameter]) - assert len(response.results) == 1 - result = response.results[0] - if result.successful: - node.get_logger().info(bcolors.OKCYAN + 'Set controller type to "' + controller_type + '" for ' + bcolors.BOLD + prefixed_controller_name + bcolors.ENDC) - else: - node.get_logger().fatal(bcolors.FAIL + 'Could not set controller type to "' + controller_type + '" for ' + bcolors.BOLD + prefixed_controller_name + bcolors.ENDC) + for controller_name in controller_names: + prefixed_controller_name = controller_name + if controller_namespace: + prefixed_controller_name = controller_namespace + "/" + controller_name + + if is_controller_loaded(node, controller_manager_name, prefixed_controller_name): + node.get_logger().warn( + bcolors.WARNING + + "Controller already loaded, skipping load_controller" + + bcolors.ENDC + ) + else: + if controller_type: + parameter = Parameter() + parameter.name = prefixed_controller_name + ".type" + parameter.value = get_parameter_value(string_value=controller_type) + + response = call_set_parameters( + node=node, node_name=controller_manager_name, parameters=[parameter] + ) + assert len(response.results) == 1 + result = response.results[0] + if result.successful: + node.get_logger().info( + bcolors.OKCYAN + + 'Set controller type to "' + + controller_type + + '" for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + else: + node.get_logger().fatal( + bcolors.FAIL + + 'Could not set controller type to "' + + controller_type + + '" for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + return 1 + + ret = load_controller(node, controller_manager_name, controller_name) + if not ret.ok: + node.get_logger().fatal( + bcolors.FAIL + + "Failed loading controller " + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + return 1 + node.get_logger().info( + bcolors.OKBLUE + + "Loaded " + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + + if param_file: + # load_parameter_file writes to stdout/stderr. Here we capture that and use node logging instead + with redirect_stdout(io.StringIO()) as f_stdout, redirect_stderr( + io.StringIO() + ) as f_stderr: + load_parameter_file( + node=node, + node_name=prefixed_controller_name, + parameter_file=param_file, + use_wildcard=True, + ) + if f_stdout.getvalue(): + node.get_logger().info(bcolors.OKCYAN + f_stdout.getvalue() + bcolors.ENDC) + if f_stderr.getvalue(): + node.get_logger().error(bcolors.FAIL + f_stderr.getvalue() + bcolors.ENDC) + node.get_logger().info( + bcolors.OKCYAN + + 'Loaded parameters file "' + + param_file + + '" for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + # TODO(destogl): use return value when upstream return value is merged + # ret = + # if ret.returncode != 0: + # Error message printed by ros2 param + # return ret.returncode + node.get_logger().info( + "Loaded " + param_file + " into " + prefixed_controller_name + ) + + if not args.load_only: + ret = configure_controller(node, controller_manager_name, controller_name) + if not ret.ok: + node.get_logger().error( + bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC + ) return 1 - ret = load_controller(node, controller_manager_name, controller_name) - if not ret.ok: - node.get_logger().fatal(bcolors.FAIL + 'Failed loading controller ' + bcolors.BOLD + prefixed_controller_name + bcolors.ENDC) - return 1 - node.get_logger().info(bcolors.OKBLUE + 'Loaded ' + bcolors.BOLD + prefixed_controller_name + bcolors.ENDC) - - if param_file: - load_parameter_file(node=node, node_name=prefixed_controller_name, parameter_file=param_file, - use_wildcard=True) - node.get_logger().info(bcolors.OKCYAN + 'Loaded parameters file "' + param_file + '" for ' + bcolors.BOLD + prefixed_controller_name + bcolors.ENDC) - # TODO(destogl): use return value when upstream return value is merged - # ret = - # if ret.returncode != 0: - # Error message printed by ros2 param - # return ret.returncode - node.get_logger().info('Loaded ' + param_file + ' into ' + prefixed_controller_name) - - if not args.load_only: - ret = configure_controller(node, controller_manager_name, controller_name) + if not args.stopped and not args.inactive and not args.activate_as_group: + ret = switch_controllers( + node, controller_manager_name, [], [controller_name], True, True, 5.0 + ) + if not ret.ok: + node.get_logger().error( + bcolors.FAIL + "Failed to activate controller" + bcolors.ENDC + ) + return 1 + + node.get_logger().info( + bcolors.OKGREEN + + "Configured and activated " + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + + if not args.stopped and not args.inactive and args.activate_as_group: + ret = switch_controllers( + node, controller_manager_name, [], controller_names, True, True, 5.0 + ) if not ret.ok: - node.get_logger().error('Failed to configure controller') + node.get_logger().error( + bcolors.FAIL + "Failed to activate the parsed controllers list" + bcolors.ENDC + ) return 1 - if not args.stopped and not args.inactive: - ret = switch_controllers( - node, - controller_manager_name, - [], - [controller_name], - True, - True, - 5.0) - if not ret.ok: - node.get_logger().error('Failed to activate controller') - return 1 - - node.get_logger().info(bcolors.OKGREEN + 'Configured and activated ' + - bcolors.BOLD + prefixed_controller_name + bcolors.ENDC) - elif args.stopped: - node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead') + node.get_logger().info( + bcolors.OKGREEN + + "Configured and activated all the parsed controllers list!" + + bcolors.ENDC + ) + if args.stopped: + node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead') if not args.unload_on_kill: return 0 @@ -252,15 +337,11 @@ def main(args=None): time.sleep(1) except KeyboardInterrupt: if not args.stopped and not args.inactive: - node.get_logger().info('Interrupt captured, deactivating and unloading controller') + node.get_logger().info("Interrupt captured, deactivating and unloading controller") + # TODO(saikishor) we might have an issue in future, if any of these controllers is in chained mode ret = switch_controllers( - node, - controller_manager_name, - [controller_name], - [], - True, - True, - 5.0) + node, controller_manager_name, controller_names, [], True, True, 5.0 + ) if not ret.ok: node.get_logger().error('Failed to deactivate controller') return 1 diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 4020e2ad06..efa385e0a5 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -28,35 +28,30 @@ def main(args=None): rclpy.init(args=args) parser = argparse.ArgumentParser() - parser.add_argument( - 'controller_name', help='Name of the controller') + parser.add_argument("controller_names", help="Name of the controller", nargs="+") parser.add_argument( '-c', '--controller-manager', help='Name of the controller manager ROS node', default='/controller_manager', required=False) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) - controller_name = args.controller_name + controller_names = args.controller_names controller_manager_name = args.controller_manager - node = Node('unspawner_' + controller_name) + node = Node("unspawner_" + controller_names[0]) try: # Ignore returncode, because message is already printed and we'll try to unload anyway ret = switch_controllers( - node, - controller_manager_name, - [controller_name], - [], - True, - True, - 5.0) - node.get_logger().info('Deactivated controller') + node, controller_manager_name, controller_names, [], True, True, 5.0 + ) + node.get_logger().info("Deactivated controller") - ret = unload_controller(node, controller_manager_name, controller_name) - if not ret.ok: - node.get_logger().info('Failed to unload controller') - return 1 - node.get_logger().info('Unloaded controller') + for controller_name in controller_names: + ret = unload_controller(node, controller_manager_name, controller_name) + if not ret.ok: + node.get_logger().info("Failed to unload controller") + return 1 + node.get_logger().info("Unloaded controller") return 0 finally: diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst index d79e730d3f..363eeb70f7 100644 --- a/controller_manager/doc/controller_chaining.rst +++ b/controller_manager/doc/controller_chaining.rst @@ -76,10 +76,10 @@ One can also think of it as an actual chain, you can not add a chain link or bre Debugging outputs ---------------------------- -Flag ``unavailable`` on reference interface does not provide much information about anything at the moment. So don't get confused by it. The reason we have it are internal implementation reasons irelevant for the usage. +Flag ``unavailable`` if the reference interface does not provide much information about anything at the moment. So don't get confused by it. The reason we have it are internal implementation reasons irelevant for the usage. Closing remarks ---------------------------- -- Maybe addition of the new controller's type ``ChainableController`` is not necessary. It would also be feasible to add implementation of ``input_interface_configuration()`` method into ``ControllerInterface`` class with default result ``interface_configuration_type::NONE``. +- Maybe addition of the new controller's type ``ChainableController`` is not necessary. It would also be feasible to add an implementation of ``input_interface_configuration()`` method into ``ControllerInterface`` class with default result ``interface_configuration_type::NONE``. diff --git a/controller_manager/doc/images/chaining_example2.png b/controller_manager/doc/images/chaining_example2.png new file mode 100644 index 0000000000..1ba49a116e Binary files /dev/null and b/controller_manager/doc/images/chaining_example2.png differ diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 69268d5ca1..ea1cc0cb13 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -50,6 +50,8 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" namespace controller_manager { @@ -81,6 +83,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC virtual ~ControllerManager() = default; + CONTROLLER_MANAGER_PUBLIC + void robot_description_callback(const std_msgs::msg::String & msg); + CONTROLLER_MANAGER_PUBLIC void init_resource_manager(const std::string & robot_description); @@ -315,6 +320,7 @@ class ControllerManager : public rclcpp::Node std::vector get_controller_names(); std::pair split_command_interface( const std::string & command_interface); + void subscribe_to_robot_description_topic(); /** * Clear request lists used when switching controllers. The lists are shared between "callback" @@ -382,6 +388,29 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /// A method to be used in the std::sort method to sort the controllers to be able to + /// execute them in a proper order + /** + * Compares the controllers ctrl_a and ctrl_b and then returns which comes first in the sequence + * + * @note The following conditions needs to be handled while ordering the controller list + * 1. The controllers that do not use any state or command interfaces are updated first + * 2. The controllers that use only the state system interfaces only are updated next + * 3. The controllers that use any of an another controller's reference interface are updated + * before the preceding controller + * 4. The controllers that use the controller's estimated interfaces are updated after the + * preceding controller + * 5. The controllers that only use the hardware command interfaces are updated last + * 6. All inactive controllers go at the end of the list + * + * \param[in] controllers list of controllers to compare their names to interface's prefix. + * + * @return true, if ctrl_a needs to execute first, else false + */ + bool controller_sorting( + const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, + const std::vector & controllers); + std::shared_ptr executor_; std::shared_ptr> loader_; @@ -500,6 +529,8 @@ class ControllerManager : public rclcpp::Node std::vector activate_command_interface_request_, deactivate_command_interface_request_; + rclcpp::Subscription::SharedPtr robot_description_subscription_; + struct SwitchParams { bool do_switch = {false}; diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 8ab2d8a7d7..1fa0f65184 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 2.33.0 + 2.35.1 Description of controller_manager Bence Magyar Denis Štogl @@ -25,6 +25,7 @@ ros2_control_test_assets ros2param ros2run + std_msgs ament_cmake_gmock diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 0024359d1e..7f3b312424 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -124,6 +124,123 @@ bool command_interface_is_reference_interface_of_controller( return true; } +/** + * A method to retrieve the names of all it's following controllers given a controller name + * For instance, for the following case + * A -> B -> C -> D + * When called with B, returns C and D + * 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 + * + * @param controller_name - Name of the controller for checking the tree + * \param[in] controllers list of controllers to compare their names to interface's prefix. + * @return list of controllers that are following the given controller in a chain. If none, return + * empty. + */ +std::vector get_following_controller_names( + const std::string controller_name, + const std::vector & controllers) +{ + std::vector following_controllers; + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (controller_it == controllers.end()) + { + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "Required controller : '%s' is not found in the controller list ", controller_name.c_str()); + + return following_controllers; + } + // If the controller is not configured, return empty + if (!(is_controller_active(controller_it->c) || is_controller_inactive(controller_it->c))) + return following_controllers; + const auto cmd_itfs = controller_it->c->command_interface_configuration().names; + for (const auto & itf : cmd_itfs) + { + controller_manager::ControllersListIterator ctrl_it; + if (command_interface_is_reference_interface_of_controller(itf, controllers, ctrl_it)) + { + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "The interface is a reference interface of controller : %s", ctrl_it->info.name.c_str()); + following_controllers.push_back(ctrl_it->info.name); + const std::vector ctrl_names = + get_following_controller_names(ctrl_it->info.name, controllers); + for (const std::string & controller : ctrl_names) + { + if ( + std::find(following_controllers.begin(), following_controllers.end(), controller) == + following_controllers.end()) + { + // Only add to the list if it doesn't exist + following_controllers.push_back(controller); + } + } + } + } + return following_controllers; +} + +/** + * A method to retrieve the names of all it's preceding controllers given a controller name + * For instance, for the following case + * A -> B -> C -> D + * When called with C, returns A and B + * 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 + * + * @param controller_name - Name of the controller for checking the tree + * \param[in] controllers list of controllers to compare their names to interface's prefix. + * @return list of controllers that are preceding the given controller in a chain. If none, return + * empty. + */ +std::vector get_preceding_controller_names( + const std::string controller_name, + const std::vector & controllers) +{ + std::vector preceding_controllers; + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (controller_it == controllers.end()) + { + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "Required controller : '%s' is not found in the controller list ", controller_name.c_str()); + return preceding_controllers; + } + for (const auto & ctrl : controllers) + { + // If the controller is not configured, then continue + if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) continue; + auto cmd_itfs = ctrl.c->command_interface_configuration().names; + for (const auto & itf : cmd_itfs) + { + auto split_pos = itf.find_first_of('/'); + if ((split_pos != std::string::npos) && (itf.substr(0, split_pos) == controller_name)) + { + preceding_controllers.push_back(ctrl.info.name); + auto ctrl_names = get_preceding_controller_names(ctrl.info.name, controllers); + for (const std::string & controller : ctrl_names) + { + if ( + std::find(preceding_controllers.begin(), preceding_controllers.end(), controller) == + preceding_controllers.end()) + { + // Only add to the list if it doesn't exist + preceding_controllers.push_back(controller); + } + } + } + } + } + return preceding_controllers; +} + } // namespace namespace controller_manager @@ -158,10 +275,16 @@ ControllerManager::ControllerManager( get_parameter("robot_description", robot_description); if (robot_description.empty()) { - throw std::runtime_error("Unable to initialize resource manager, no robot description found."); + subscribe_to_robot_description_topic(); + } + else + { + RCLCPP_WARN( + get_logger(), + "[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_resource_manager(robot_description); init_services(); } @@ -183,9 +306,54 @@ ControllerManager::ControllerManager( { RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } + + if (!resource_manager_->is_urdf_already_loaded()) + { + subscribe_to_robot_description_topic(); + } init_services(); } +void ControllerManager::subscribe_to_robot_description_topic() +{ + // set QoS to transient local to get messages that have already been published + // (if robot state publisher starts before controller manager) + RCLCPP_INFO( + get_logger(), "Subscribing to '~/robot_description' topic for robot description file."); + robot_description_subscription_ = create_subscription( + "~/robot_description", rclcpp::QoS(1).transient_local(), + std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); +} + +void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) +{ + RCLCPP_INFO(get_logger(), "Received robot description file."); + RCLCPP_DEBUG( + get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); + // TODO(Manuel): errors should probably be caught since we don't want controller_manager node + // to die if a non valid urdf is passed. However, should maybe be fine tuned. + try + { + if (resource_manager_->is_urdf_already_loaded()) + { + RCLCPP_WARN( + get_logger(), + "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " + "description file."); + return; + } + init_resource_manager(robot_description.data.c_str()); + } + catch (std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + get_logger(), + "The published robot description file (urdf) seems not to be genuine. The following error " + "was caught:" + << e.what()); + } +} + void ControllerManager::init_resource_manager(const std::string & robot_description) { // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong... @@ -512,10 +680,35 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); + } + + // Now let's reorder the controllers + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); + const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); - // TODO(destogl): check and resort controllers in the vector + // Copy all controllers from the 'from' list to the 'to' list + to = from; + + // Reordering the controllers + std::stable_sort( + to.begin(), to.end(), + std::bind( + &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, + to)); + + RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); + for (const auto & ctrl : to) + { + RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str()); } + // switch lists + rt_controllers_wrapper_.switch_updated_list(guard); + // clear unused list + rt_controllers_wrapper_.get_unused_list(guard).clear(); + return controller_interface::return_type::OK; } @@ -976,6 +1169,7 @@ controller_interface::return_type ControllerManager::switch_controller( controller.info.claimed_interfaces.clear(); } } + // switch lists rt_controllers_wrapper_.switch_updated_list(guard); // clear unused list @@ -2169,6 +2363,113 @@ controller_interface::return_type ControllerManager::check_preceeding_controller } } return controller_interface::return_type::OK; +} + +bool ControllerManager::controller_sorting( + const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, + const std::vector & controllers) +{ + // If the neither of the controllers are configured, then return false + if (!((is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) && + (is_controller_active(ctrl_b.c) || is_controller_inactive(ctrl_b.c)))) + { + if (is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) return true; + return false; + } + + const std::vector cmd_itfs = ctrl_a.c->command_interface_configuration().names; + const std::vector state_itfs = ctrl_a.c->state_interface_configuration().names; + if (cmd_itfs.empty() || !ctrl_a.c->is_chainable()) + { + // The case of the controllers that don't have any command interfaces. For instance, + // joint_state_broadcaster + // 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 + { + auto following_ctrls = get_following_controller_names(ctrl_a.info.name, controllers); + if (following_ctrls.empty()) return false; + // If the ctrl_b is any of the following controllers of ctrl_a, then place ctrl_a before ctrl_b + if ( + std::find(following_ctrls.begin(), following_ctrls.end(), ctrl_b.info.name) != + following_ctrls.end()) + return true; + else + { + auto ctrl_a_preceding_ctrls = get_preceding_controller_names(ctrl_a.info.name, controllers); + // This is to check that the ctrl_b is in the preceding controllers list of ctrl_a - This + // check is useful when there is a chained controller branching, but they belong to same + // branch + if ( + std::find(ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end(), ctrl_b.info.name) != + ctrl_a_preceding_ctrls.end()) + { + return false; + } + + // This is to handle the cases where, the parsed ctrl_a and ctrl_b are not directly related + // but might have a common parent - happens in branched chained controller + auto ctrl_b_preceding_ctrls = get_preceding_controller_names(ctrl_b.info.name, controllers); + std::sort(ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end()); + std::sort(ctrl_b_preceding_ctrls.begin(), ctrl_b_preceding_ctrls.end()); + std::list intersection; + std::set_intersection( + ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end(), + ctrl_b_preceding_ctrls.begin(), ctrl_b_preceding_ctrls.end(), + std::back_inserter(intersection)); + if (!intersection.empty()) + { + // If there is an intersection, then there is a common parent controller for both ctrl_a and + // ctrl_b + return true; + } + + // If there is no common parent, then they belong to 2 different sets + auto following_ctrls_b = get_following_controller_names(ctrl_b.info.name, controllers); + if (following_ctrls_b.empty()) return true; + auto find_first_element = [&](const auto & controllers_list) + { + auto it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controllers_list.back())); + if (it != controllers.end()) + { + int dist = std::distance(controllers.begin(), it); + return dist; + } + }; + const int ctrl_a_chain_first_controller = find_first_element(following_ctrls); + const int ctrl_b_chain_first_controller = find_first_element(following_ctrls_b); + if (ctrl_a_chain_first_controller < ctrl_b_chain_first_controller) return true; + } + + // If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be + // infront of ctrl_a + // TODO(saikishor): deal with the state interface chaining in the sorting algorithm + auto state_it = std::find_if( + state_itfs.begin(), state_itfs.end(), + [ctrl_b](auto itf) + { + auto index = itf.find_first_of('/'); + return ((index != std::string::npos) && (itf.substr(0, index) == ctrl_b.info.name)); + }); + if (state_it != state_itfs.end()) + { + return false; + } + + // The rest of the cases, basically end up at the end of the list + return false; + } }; } // namespace controller_manager diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index b6e6acac0e..78c3fcb06b 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -22,15 +22,20 @@ #include #include #include +#include #include #include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_manager.hpp" +#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp/utilities.hpp" +#include "std_msgs/msg/string.hpp" + #include "ros2_control_test_assets/descriptions.hpp" #include "test_controller_failed_init/test_controller_failed_init.hpp" @@ -60,21 +65,51 @@ template class ControllerManagerFixture : public ::testing::Test { public: + explicit ControllerManagerFixture( + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, + const bool & pass_urdf_as_parameter = false) + : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) + { + executor_ = std::make_shared(); + // We want to be able to create a ResourceManager where no urdf file has been passed to + if (robot_description_.empty()) + { + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + } + else + { + // can be removed later, needed if we want to have the deprecated way of passing the robot + // description file to the controller manager covered by tests + if (pass_urdf_as_parameter_) + { + cm_ = std::make_shared( + std::make_unique(robot_description_, true, true), + executor_, TEST_CM_NAME); + } + else + { + // TODO(Manuel) : passing via topic not working in test setup, tested cm does + // not receive msg. Have to check this... + + // this is just a workaround to skip passing + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + // mimic topic call + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); + } + } + } + static void SetUpTestCase() { rclcpp::init(0, nullptr); } static void TearDownTestCase() { rclcpp::shutdown(); } - void SetUp() - { - executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf, true, true), - executor_, TEST_CM_NAME); - run_updater_ = false; - } + void SetUp() override { run_updater_ = false; } - void TearDown() { stopCmUpdater(); } + void TearDown() override { stopCmUpdater(); } void startCmUpdater() { @@ -120,6 +155,8 @@ class ControllerManagerFixture : public ::testing::Test std::thread updater_; bool run_updater_; + const std::string robot_description_; + const bool pass_urdf_as_parameter_; }; class TestControllerManagerSrvs diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 6db4cfd1b2..a5566ee9b5 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -104,8 +104,11 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); - cm_->configure_controller(TEST_CONTROLLER2_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + cm_->configure_controller(TEST_CONTROLLER2_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); @@ -217,7 +220,10 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) test_controller->get_node()->set_parameter({"update_rate", 4}); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); @@ -296,7 +302,10 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd rclcpp::Parameter update_rate_parameter("update_rate", static_cast(ctrl_update_rate)); test_controller->get_node()->set_parameter(update_rate_parameter); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); @@ -390,7 +399,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 68fe01946e..30849a3ba3 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -261,27 +261,28 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) // get controller list after configure result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(2u, result->controller.size()); + // At this stage, the controllers are already reordered // check chainable controller ASSERT_EQ(result->controller[0].state, "inactive"); ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u); - ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 1u); + ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 3u); ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u); - ASSERT_EQ(result->controller[0].is_chainable, true); + ASSERT_EQ(result->controller[0].is_chainable, false); ASSERT_EQ(result->controller[0].is_chained, false); - ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u); - ASSERT_EQ("joint1/position", result->controller[0].reference_interfaces[0]); - ASSERT_EQ("joint1/velocity", result->controller[0].reference_interfaces[1]); + ASSERT_EQ(result->controller[0].reference_interfaces.size(), 0u); + ASSERT_EQ(result->controller[0].chain_connections.size(), 1u); - ASSERT_EQ(result->controller[0].chain_connections.size(), 0u); // check test controller ASSERT_EQ(result->controller[1].state, "inactive"); ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 0u); - ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 3u); + ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 1u); ASSERT_EQ(result->controller[1].required_state_interfaces.size(), 2u); - ASSERT_EQ(result->controller[1].is_chainable, false); + ASSERT_EQ(result->controller[1].is_chainable, true); ASSERT_EQ(result->controller[1].is_chained, false); - ASSERT_EQ(result->controller[1].reference_interfaces.size(), 0u); - ASSERT_EQ(result->controller[1].chain_connections.size(), 1u); + ASSERT_EQ(result->controller[1].reference_interfaces.size(), 2u); + ASSERT_EQ(result->controller[1].chain_connections.size(), 0u); + ASSERT_EQ("joint1/position", result->controller[1].reference_interfaces[0]); + ASSERT_EQ("joint1/velocity", result->controller[1].reference_interfaces[1]); // activate controllers cm_->switch_controller( {test_chainable_controller::TEST_CONTROLLER_NAME}, {}, @@ -291,19 +292,19 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); // get controller list after activate result = call_service_and_wait(*client, request, srv_executor); - // check chainable controller - ASSERT_EQ(result->controller[0].state, "active"); - ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 1u); - ASSERT_EQ(result->controller[0].is_chained, true); // check test controller + ASSERT_EQ(result->controller[0].state, "active"); + ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 3u); + // check chainable controller ASSERT_EQ(result->controller[1].state, "active"); - ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 3u); + ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 1u); + ASSERT_EQ(result->controller[1].is_chained, true); ASSERT_EQ( test_chainable_controller::TEST_CONTROLLER_NAME, - result->controller[1].chain_connections[0].name); - ASSERT_EQ(2u, result->controller[1].chain_connections[0].reference_interfaces.size()); - ASSERT_EQ("joint1/position", result->controller[1].chain_connections[0].reference_interfaces[0]); - ASSERT_EQ("joint1/velocity", result->controller[1].chain_connections[0].reference_interfaces[1]); + result->controller[0].chain_connections[0].name); + ASSERT_EQ(2u, result->controller[0].chain_connections[0].reference_interfaces.size()); + ASSERT_EQ("joint1/position", result->controller[0].chain_connections[0].reference_interfaces[0]); + ASSERT_EQ("joint1/velocity", result->controller[0].chain_connections[0].reference_interfaces[1]); } TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) @@ -482,3 +483,1024 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, cm_->get_loaded_controllers()[0].c->get_state().id()); } + +TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) +{ + /// The simulated controller chaining is: + /// test_controller_name -> chain_ctrl_5 -> chain_ctrl_4 -> chain_ctrl_3 -> chain_ctrl_2 -> + /// chain_ctrl_1 + /// + /// 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"; + 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_chained_controller_4 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_3) + "/joint1/position"}}; + 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({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_5 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_4) + "/joint1/position"}}; + 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({"joint1/position", "joint1/velocity"}); + + // create non-chained controller + auto test_controller = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_5) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_5) + "/joint1/velocity", "joint2/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller->set_command_interface_configuration(cmd_cfg); + test_controller->set_state_interface_configuration(state_cfg); + // add controllers + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + 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_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_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_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_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(6u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_4); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_1); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_3); + ASSERT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_5); + ASSERT_EQ(result->controller[4].name, TEST_CHAINED_CONTROLLER_2); + // check test controller + ASSERT_EQ(result->controller[5].name, "test_controller_name"); + + // configure controllers + for (const auto & controller : + {TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, + TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_1, + test_controller::TEST_CONTROLLER_NAME}) + cm_->configure_controller(controller); + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(6u, result->controller.size()); + + // reordered controllers + ASSERT_EQ(result->controller[0].name, "test_controller_name"); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_5); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_4); + ASSERT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_3); + ASSERT_EQ(result->controller[4].name, TEST_CHAINED_CONTROLLER_2); + ASSERT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_1); + RCLCPP_ERROR(srv_node->get_logger(), "Check successful!"); +} + +TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) +{ + /// The simulated controller chain branching is: + /// test_controller_name -> chain_ctrl_7 -> chain_ctrl_6 -> chain_ctrl_2 -> chain_ctrl_1 + /// & + /// chain_ctrl_6 -> chain_ctrl_5 -> chain_ctrl_4 -> chain_ctrl_3 + /// + /// 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"; + 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", "joint2/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, {"joint2/velocity"}}; + 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({"joint2/velocity"}); + + auto test_chained_controller_4 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_3) + "/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_2) + "/joint1/position", + 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({"joint1/position", "joint2/velocity"}); + + auto test_chained_controller_7 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_6) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_6) + "/joint2/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({"joint1/position", "joint2/velocity"}); + + // create non-chained controller + auto test_controller = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_7) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_7) + "/joint2/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint2/velocity"}}; + test_controller->set_command_interface_configuration(cmd_cfg); + test_controller->set_state_interface_configuration(state_cfg); + // add controllers + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + 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_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_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_6, TEST_CHAINED_CONTROLLER_6, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, + 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(8u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_4); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_1); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_3); + ASSERT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_5); + ASSERT_EQ(result->controller[4].name, TEST_CHAINED_CONTROLLER_6); + ASSERT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_2); + // check test controller + ASSERT_EQ(result->controller[6].name, "test_controller_name"); + ASSERT_EQ(result->controller[7].name, TEST_CHAINED_CONTROLLER_7); + + // configure controllers + for (const auto & controller : + {TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, + TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_1, + TEST_CHAINED_CONTROLLER_6, test_controller::TEST_CONTROLLER_NAME}) + cm_->configure_controller(controller); + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(8u, result->controller.size()); + + // reordered controllers + ASSERT_EQ(result->controller[0].name, "test_controller_name"); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_7); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_6); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int + { + 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_1_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_1); + auto ctrl_2_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_2); + auto ctrl_3_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_3); + auto ctrl_4_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_4); + auto ctrl_5_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_5); + auto ctrl_6_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_6); + + // Extra check to see that they are index only after their parent controller (ctrl_6) + ASSERT_GT(ctrl_3_pos, ctrl_6_pos); + ASSERT_GT(ctrl_1_pos, ctrl_6_pos); + + // first branch + ASSERT_GT(ctrl_2_pos, ctrl_6_pos); + ASSERT_GT(ctrl_1_pos, ctrl_2_pos); + + // second branch + ASSERT_GT(ctrl_5_pos, ctrl_6_pos); + ASSERT_GT(ctrl_4_pos, ctrl_5_pos); + ASSERT_GT(ctrl_3_pos, ctrl_4_pos); +} + +TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) +{ + /// 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 + /// + /// 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_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"}); + + // 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); + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + + // check chainable controller + ASSERT_EQ(10u, 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_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}; + for (const auto & controller : ctrls_order) cm_->configure_controller(controller); + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(10u, result->controller.size()); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int + { + 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_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); +} + +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_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp new file mode 100644 index 0000000000..102cbdfbd4 --- /dev/null +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -0,0 +1,88 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" + +#include "ros2_control_test_assets/descriptions.hpp" + +class TestControllerManagerWithTestableCM; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerManagerWithTestableCM; + + FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + +public: + TestableControllerManager( + std::unique_ptr resource_manager, + std::shared_ptr executor, + const std::string & manager_node_name = "controller_manager", + const std::string & namespace_ = "") + : controller_manager::ControllerManager( + std::move(resource_manager), executor, manager_node_name, namespace_) + { + } +}; + +class TestControllerManagerWithTestableCM +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +public: + // create cm with no urdf + TestControllerManagerWithTestableCM() + : ControllerManagerFixture("", false) + { + } +}; + +TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) +{ + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); +} + +TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) +{ + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + // mimic callback + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_urdf; + cm_->robot_description_callback(msg); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); +} + +TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed) +{ + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + // mimic callback + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf; + cm_->robot_description_callback(msg); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); +} + +INSTANTIATE_TEST_SUITE_P( + test_best_effort, TestControllerManagerWithTestableCM, testing::Values(best_effort)); diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 0d798a1b79..31d2cadcf7 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -42,6 +42,8 @@ class TestableTestChainableController : public test_chainable_controller::TestCh FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_auto_switch_to_chained_mode); + FRIEND_TEST( + TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order); }; class TestableControllerManager : public controller_manager::ControllerManager @@ -65,6 +67,8 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_auto_switch_to_chained_mode); + FRIEND_TEST( + TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order); public: TestableControllerManager( @@ -610,11 +614,129 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); } -// TODO(destogl): Add test case with controllers added in "random" order +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order) +{ + SetupControllers(); -// TODO(destogl): Think about strictness and chaining controllers -// new value: "START_DOWNSTREAM_CTRLS" --> start "downstream" controllers in a controllers chain -// + // add all controllers in random order to test the sorting + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES); + + EXPECT_THROW( + cm_->resource_manager_->make_controller_reference_interfaces_available( + POSITION_TRACKING_CONTROLLER), + std::out_of_range); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + // activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER + // (otherwise, interface will be missing) + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 3u); + + // Diff-Drive Controller claims the reference interfaces of PID controllers + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 3u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 5u); + + // Position-Tracking Controller uses reference interfaces of Diff-Drive Controller + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + // 'rot_z' reference interface is not claimed + for (const auto & interface : {"diff_drive_controller/rot_z"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + ASSERT_EQ(diff_drive_controller->internal_counter, 3u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); + + // update controllers + std::vector reference = {32.0, 128.0}; + + // update 'Position Tracking' controller + for (auto & value : diff_drive_controller->reference_interfaces_) + { + ASSERT_EQ(value, 0.0); // default reference values are 0.0 + } + position_tracking_controller->external_commands_for_testing_[0] = reference[0]; + position_tracking_controller->external_commands_for_testing_[1] = reference[1]; + position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(position_tracking_controller->internal_counter, 2u); + + ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller + ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through + + // update 'Diff Drive' Controller + diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(diff_drive_controller->internal_counter, 4u); + // default reference values are 0.0 - they should be changed now + EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 + EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 + ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); + ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + + // update PID controllers that are writing to hardware + pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 8u); + pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 6u); + + // update hardware ('read' is sufficient for test hardware) + cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // 32 - 0 + EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); + // 32 / 2 + EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + + // 128 - 0 + EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); + // 128 / 2 + EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + + // update all controllers at once and see that all have expected values --> also checks the order + // of controller execution + + reference = {1024.0, 4096.0}; + UpdateAllControllerAndCheck(reference, 3u); +} INSTANTIATE_TEST_SUITE_P( test_strict_best_effort, TestControllerChainingWithControllerManager, diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index ca2f7f6b1c..390a7365f0 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -162,7 +162,10 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); // Activate configured controller - cm_->configure_controller(controller_name1); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(controller_name1); + } start_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } @@ -246,7 +249,10 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure test_controller->cleanup_calls = &cleanup_calls; // Configure from inactive state test_controller->simulate_cleanup_failure = false; - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + } ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); EXPECT_EQ(1u, cleanup_calls); } @@ -421,7 +427,10 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); - cm_->configure_controller(controller_name2); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(controller_name2); + } // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1"); diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 12d5566889..7c03a8862d 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -125,6 +125,93 @@ TEST_F(TestLoadController, spawner_test_type_in_param) ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } +TEST_F(TestLoadController, multi_ctrls_test_type_in_param) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager"), 0); + + auto validate_loaded_controllers = [&]() + { + auto loaded_controllers = cm_->get_loaded_controllers(); + for (size_t i = 0; i < loaded_controllers.size(); i++) + { + auto ctrl = loaded_controllers[i]; + const std::string controller_name = "ctrl_" + std::to_string(i + 1); + + RCLCPP_ERROR(rclcpp::get_logger("test_controller_manager"), "%s", controller_name.c_str()); + auto it = std::find_if( + loaded_controllers.begin(), loaded_controllers.end(), + [&](const auto & controller) { return controller.info.name == controller_name; }); + ASSERT_TRUE(it != loaded_controllers.end()); + ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + }; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + { + validate_loaded_controllers(); + } + + // Try to spawn again multiple but one of them is already active, it should fail because already + // active + EXPECT_NE(call_spawner("ctrl_1 ctrl_3 -c test_controller_manager"), 0) + << "Cannot configure from active"; + + std::vector start_controllers = {}; + std::vector stop_controllers = {"ctrl_1"}; + cm_->switch_controller( + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + + // We should be able to reconfigure and activate a configured controller + EXPECT_EQ(call_spawner("ctrl_1 ctrl_3 -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + validate_loaded_controllers(); + } + + stop_controllers = {"ctrl_1", "ctrl_2"}; + cm_->switch_controller( + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + for (auto ctrl : stop_controllers) + { + cm_->unload_controller(ctrl); + cm_->load_controller(ctrl); + } + + // We should be able to reconfigure and activate am unconfigured loaded controller + EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + validate_loaded_controllers(); + } + + // Unload and reload + EXPECT_EQ(call_unspawner("ctrl_1 ctrl_3 -c test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been unloaded"; + EXPECT_EQ(call_spawner("ctrl_1 ctrl_3 -c test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded"; + { + validate_loaded_controllers(); + } + + // Now unload everything and load them as a group in a single operation + EXPECT_EQ(call_unspawner("ctrl_1 ctrl_2 ctrl_3 -c test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul) << "Controller should have been unloaded"; + EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 ctrl_3 -c test_controller_manager --activate-as-group"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded"; + { + validate_loaded_controllers(); + } +} + TEST_F(TestLoadController, spawner_test_type_in_arg) { // Provide controller type via -t argument diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index ec22935d97..5a111d2108 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.35.1 (2023-11-27) +------------------- + +2.35.0 (2023-11-14) +------------------- + +2.34.0 (2023-11-08) +------------------- + 2.33.0 (2023-10-11) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index ae0956913d..3fb2555734 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 2.33.0 + 2.35.1 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/doc/index.rst b/doc/index.rst index 196ea93285..91f965d601 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -6,6 +6,8 @@ ros2_control ################# +This is the documentation of the ros2_control framework core. + `GitHub Repository `_ ================= @@ -14,15 +16,12 @@ API Documentation API documentation is parsed by doxygen and can be found `here <../../api/index.html>`_ - - ========= Features ========= * :ref:`Command Line Interface (CLI) ` - ======== Concepts ======== @@ -31,5 +30,6 @@ Concepts :titlesonly: Controller Manager <../controller_manager/doc/userdoc.rst> + Controller Chaining / Cascade Control <../controller_manager/doc/controller_chaining.rst> Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst> Mock Components <../hardware_interface/doc/mock_components_userdoc.rst> diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index dd4678b623..97960d188e 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 `_) +* Contributors: mergify[bot] + +2.34.0 (2023-11-08) +------------------- + 2.33.0 (2023-10-11) ------------------- * [MockHardware] Added dynamic simulation functionality. (`#1028 `_) (`#1125 `_) diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 5bcedb19ea..fb527d39d2 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -27,7 +27,12 @@ Features: Parameters ,,,,,,,,,, -fake_sensor_commands (optional; boolean; default: false) +disable_commands (optional; boolean; default: false) + Disables mirroring commands to states. + This option is helpful to simulate an erroneous connection to the hardware when nothing breaks, but suddenly there is no feedback from a hardware interface. + Or it can help you to test your setup when the hardware is running without feedback, i.e., in open loop configuration. + +mock_sensor_commands (optional; boolean; default: false) Creates fake command interfaces for faking sensor measurements with an external command. Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world. diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 8b2914a381..44dcb57e72 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -83,6 +83,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void load_urdf(const std::string & urdf, bool validate_interfaces = true); + /** + * @brief if the resource manager load_urdf(...) function has been called this returns true. + * We want to permit to load the urdf later on but we currently don't want to permit multiple + * calls to load_urdf (reloading/loading different urdf). + * + * @return true if resource manager's load_urdf() has been already called. + * @return false if resource manager's load_urdf() has not been yet called. + */ + bool is_urdf_already_loaded() const; + /// Claim a state interface given its key. /** * The resource is claimed as long as being in scope. @@ -108,12 +118,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. @@ -187,8 +191,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /** * Return list of cached controller names that use the hardware with name \p hardware_name. * - * \param[in] hardware_name the name of the hardware for which cached controllers should be returned. - * \returns list of cached controller names that depend on hardware with name \p hardware_name. + * \param[in] hardware_name the name of the hardware for which cached controllers should be + * returned. \returns list of cached controller names that depend on hardware with name \p + * hardware_name. */ std::vector get_cached_controllers_to_hardware(const std::string & hardware_name); @@ -228,13 +233,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. @@ -390,6 +388,19 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void activate_all_components(); + /// 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; + private: void validate_storage(const std::vector & hardware_info) const; @@ -405,6 +416,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // 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; }; } // namespace hardware_interface diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index a8bbbae9ca..c80ca84b76 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 2.33.0 + 2.35.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..694e92355e 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -218,6 +218,7 @@ const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -234,6 +235,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index f34c08c9a4..08132dbdf6 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -375,6 +375,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_; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e71a41139b..dd43850b4c 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -675,8 +675,10 @@ ResourceManager::ResourceManager( } } +// CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { + is_urdf_loaded__ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; @@ -715,6 +717,9 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac resource_storage_->systems_.size()); } +bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } + +// CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) { if (!state_interface_is_available(key)) @@ -726,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; @@ -737,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_); @@ -759,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) { @@ -768,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) { @@ -785,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) { @@ -807,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 = @@ -906,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; @@ -917,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_); @@ -940,16 +941,6 @@ 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) { @@ -984,8 +975,8 @@ 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_) @@ -1004,6 +995,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) @@ -1049,6 +1041,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) @@ -1076,6 +1069,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) { @@ -1159,6 +1153,7 @@ return_type ResourceManager::set_component_state( return result; } +// CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -1186,6 +1181,7 @@ HardwareReadWriteStatus ResourceManager::read( return read_write_status; } +// CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::write( const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -1212,6 +1208,39 @@ HardwareReadWriteStatus ResourceManager::write( 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 { @@ -1284,7 +1313,9 @@ void ResourceManager::validate_storage( } } -// Temporary method to keep old interface and reduce framework changes in PRs +// END: private methods + +// Temporary method to keep old interface and reduce framework changes in the PRs void ResourceManager::activate_all_components() { using lifecycle_msgs::msg::State; diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 036237221c..ade9b8781a 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -195,6 +195,7 @@ const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_st return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" 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..f8703a47bc 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -214,6 +214,7 @@ const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_st return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" return_type result = return_type::ERROR; if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || @@ -230,6 +231,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + // TODO(destogl): discuss what should be default return value, e.g., "NOT_EXECUTED" 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_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 47dd9f0d32..5f9c09e95e 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; } diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 73659ca506..6ed9254393 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -19,6 +19,8 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rcutils/logging_macros.h" + using hardware_interface::CommandInterface; using hardware_interface::return_type; using hardware_interface::StateInterface; @@ -54,6 +56,7 @@ class TestSystem : public SystemInterface std::vector export_command_interfaces() override { + RCUTILS_LOG_INFO_NAMED("test_system", "Exporting configuration interfaces."); std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { @@ -80,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; } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index eca8bbf40a..8b56fda85b 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -45,7 +45,7 @@ using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; -class TestResourceManager : public ::testing::Test +class ResourceManagerTest : public ::testing::Test { public: static void SetUpTestCase() {} @@ -53,6 +53,31 @@ class TestResourceManager : public ::testing::Test void SetUp() {} }; +// Forward declaration +namespace hardware_interface +{ +class ResourceStorage; +} + +class TestableResourceManager : public hardware_interface::ResourceManager +{ +public: + friend ResourceManagerTest; + + FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_manual_validation); + FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); + FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); + FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); + + TestableResourceManager() : hardware_interface::ResourceManager() {} + + TestableResourceManager( + const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) + { + } +}; + std::vector set_components_state( hardware_interface::ResourceManager & rm, const std::vector & components, const uint8_t state_id, const std::string & state_name) @@ -73,7 +98,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, @@ -81,7 +106,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, @@ -89,7 +114,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, @@ -97,7 +122,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, @@ -105,34 +130,33 @@ 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, hardware_interface::lifecycle_state_names::FINALIZED); }; -TEST_F(TestResourceManager, initialization_empty) +TEST_F(ResourceManagerTest, initialization_empty) { - ASSERT_ANY_THROW(hardware_interface::ResourceManager rm("")); + ASSERT_ANY_THROW(TestableResourceManager rm("")); } -TEST_F(TestResourceManager, initialization_with_urdf) +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(TestResourceManager, post_initialization_with_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(TestResourceManager, initialization_with_urdf_manual_validation) +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()); @@ -153,28 +177,26 @@ TEST_F(TestResourceManager, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } -TEST_F(TestResourceManager, initialization_with_wrong_urdf) +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); } // 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); } } -TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) +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) @@ -190,9 +212,37 @@ TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) } } -TEST_F(TestResourceManager, resource_claiming) +TEST_F(ResourceManagerTest, no_load_urdf_function_called) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.is_urdf_already_loaded()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) +{ + TestableResourceManager rm; + EXPECT_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); + ASSERT_TRUE(rm.is_urdf_already_loaded()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid) +{ + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.is_urdf_already_loaded()); +} + +TEST_F(ResourceManagerTest, can_load_urdf_later) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.is_urdf_already_loaded()); + rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.is_urdf_already_loaded()); +} + +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); @@ -301,10 +351,10 @@ class ExternalComponent : public hardware_interface::ActuatorInterface } }; -TEST_F(TestResourceManager, post_initialization_add_components) +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); @@ -345,9 +395,9 @@ TEST_F(TestResourceManager, post_initialization_add_components) EXPECT_NO_THROW(rm.claim_command_interface("external_joint/external_command_interface")); } -TEST_F(TestResourceManager, default_prepare_perform_switch) +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); @@ -379,9 +429,9 @@ const auto command_mode_urdf = std::string(ros2_control_test_assets::urdf_head) std::string(hardware_resources_command_modes) + std::string(ros2_control_test_assets::urdf_tail); -TEST_F(TestResourceManager, custom_prepare_perform_switch) +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"}; @@ -415,9 +465,9 @@ TEST_F(TestResourceManager, custom_prepare_perform_switch) EXPECT_FALSE(rm.perform_command_mode_switch(empty_keys, legal_keys_position)); } -TEST_F(TestResourceManager, resource_status) +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(); @@ -487,9 +537,9 @@ TEST_F(TestResourceManager, resource_status) status_map[TEST_SYSTEM_HARDWARE_NAME].state_interfaces, TEST_SYSTEM_HARDWARE_STATE_INTERFACES); } -TEST_F(TestResourceManager, lifecycle_all_resources) +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 { @@ -630,9 +680,9 @@ TEST_F(TestResourceManager, lifecycle_all_resources) } } -TEST_F(TestResourceManager, lifecycle_individual_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 { @@ -842,10 +892,10 @@ TEST_F(TestResourceManager, lifecycle_individual_resources) } } -TEST_F(TestResourceManager, resource_availability_and_claiming_in_lifecycle) +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) @@ -889,50 +939,44 @@ TEST_F(TestResourceManager, 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 @@ -949,28 +993,23 @@ TEST_F(TestResourceManager, 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 @@ -988,24 +1027,20 @@ TEST_F(TestResourceManager, 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 @@ -1021,20 +1056,20 @@ TEST_F(TestResourceManager, 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- @@ -1043,26 +1078,23 @@ TEST_F(TestResourceManager, 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: @@ -1084,22 +1116,20 @@ TEST_F(TestResourceManager, 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 @@ -1119,26 +1149,23 @@ TEST_F(TestResourceManager, 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 @@ -1159,27 +1186,23 @@ TEST_F(TestResourceManager, 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 @@ -1199,26 +1222,26 @@ TEST_F(TestResourceManager, 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(TestResourceManager, managing_controllers_reference_interfaces) +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"}; @@ -1331,3 +1354,265 @@ TEST_F(TestResourceManager, 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 7b9139698e..1a4681366c 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.35.1 (2023-11-27) +------------------- + +2.35.0 (2023-11-14) +------------------- + +2.34.0 (2023-11-08) +------------------- + 2.33.0 (2023-10-11) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 3ac8a32ffb..947b47a215 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 2.33.0 + 2.35.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 ba9cd0f390..d00718600a 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.35.1 (2023-11-27) +------------------- + +2.35.0 (2023-11-14) +------------------- + +2.34.0 (2023-11-08) +------------------- + 2.33.0 (2023-10-11) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 3bf84197d9..fe7478493a 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 2.33.0 + 2.35.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 61e40b8513..4ed5063edf 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.35.1 (2023-11-27) +------------------- + +2.35.0 (2023-11-14) +------------------- + +2.34.0 (2023-11-08) +------------------- + 2.33.0 (2023-10-11) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 03f484e9f2..23b4dca29e 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 2.33.0 + 2.35.1 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 2ab3ce18e8..c53b7469ab 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.35.1 (2023-11-27) +------------------- + +2.35.0 (2023-11-14) +------------------- + +2.34.0 (2023-11-08) +------------------- + 2.33.0 (2023-10-11) ------------------- * Fix doc of load_controller (`#1135 `_) diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 83ebdc472b..dec0c6ef9c 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 2.33.0 + 2.35.1 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 06745c30d7..a0a1638643 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version='2.33.0', + version='2.35.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 e08ff844cb..71cb52d452 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.35.1 (2023-11-27) +------------------- + +2.35.0 (2023-11-14) +------------------- + +2.34.0 (2023-11-08) +------------------- + 2.33.0 (2023-10-11) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index bcaedcade5..d4bb2ec101 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 2.33.0 + 2.35.1 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 606dcf04f2..da02a80fb5 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version='2.33.0', + version='2.35.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 60cce5731d..2496a59123 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.35.1 (2023-11-27) +------------------- + +2.35.0 (2023-11-14) +------------------- + +2.34.0 (2023-11-08) +------------------- + 2.33.0 (2023-10-11) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 5acedf7a48..965f79d308 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 2.33.0 + 2.35.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