From 62fe474d417dad0dcc8da8a4d135ee01eeac7dcc Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 30 Apr 2024 07:06:05 +0100 Subject: [PATCH 1/8] Update changelogs --- controller_interface/CHANGELOG.rst | 5 +++++ controller_manager/CHANGELOG.rst | 6 ++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 6 ++++++ hardware_interface_testing/CHANGELOG.rst | 5 +++++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 5 +++++ ros2controlcli/CHANGELOG.rst | 6 ++++++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 6 ++++++ 11 files changed, 51 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 3ed820dfe5..5dd22b111c 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* return the proper const object of the pointer in the const method (`#1494 `_) +* Contributors: Sai Kishor Kothakota + 4.8.0 (2024-03-27) ------------------ * generate version.h file per package using the ament_generate_version_header (`#1449 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 53005b3800..2fdeef4860 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Deactivate the controllers when they return error similar to hardware (`#1499 `_) +* Component parser: Get mimic information from URDF (`#1256 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.8.0 (2024-03-27) ------------------ * generate version.h file per package using the ament_generate_version_header (`#1449 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 11d3ba2989..e3c9f7216c 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-03-27) ------------------ diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 1cb22a501a..3364794961 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing calculate_dynamics (`#1498 `_) +* Component parser: Get mimic information from URDF (`#1256 `_) +* Contributors: Christoph Fröhlich + 4.8.0 (2024-03-27) ------------------ * generate version.h file per package using the ament_generate_version_header (`#1449 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 58f6edbcdb..3b657792e0 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Component parser: Get mimic information from URDF (`#1256 `_) +* Contributors: Christoph Fröhlich + 4.8.0 (2024-03-27) ------------------ diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index e318e1d3c0..2a5ec53618 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-03-27) ------------------ * generate version.h file per package using the ament_generate_version_header (`#1449 `_) diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 1ee75204b3..23f78a8d34 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-03-27) ------------------ diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 43250ac2d4..63a4912237 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Component parser: Get mimic information from URDF (`#1256 `_) +* Contributors: Christoph Fröhlich + 4.8.0 (2024-03-27) ------------------ diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 19ff351c64..f0b7b559f8 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [CI] Specify runner/container images and codecov for joint_limits (`#1504 `_) +* [CLI] Add `set_hardware_component_state` verb (`#1248 `_) +* Contributors: Christoph Fröhlich + 4.8.0 (2024-03-27) ------------------ diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index a96091ae2a..533c88149d 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.8.0 (2024-03-27) ------------------ * Fix rqt_controller_manager for non-humble (`#1454 `_) diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 70ac550955..dd01f8140b 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* rosdoc2 for transmission_interface (`#1496 `_) +* Component parser: Get mimic information from URDF (`#1256 `_) +* Contributors: Christoph Fröhlich + 4.8.0 (2024-03-27) ------------------ * generate version.h file per package using the ament_generate_version_header (`#1449 `_) From aadcc45c99ff3534cca7eb5b73d44ee68e38d7e6 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 30 Apr 2024 07:06:31 +0100 Subject: [PATCH 2/8] 4.9.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- hardware_interface_testing/CHANGELOG.rst | 4 ++-- hardware_interface_testing/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 24 files changed, 35 insertions(+), 35 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 5dd22b111c..1688434cfc 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-04-30) +------------------ * return the proper const object of the pointer in the const method (`#1494 `_) * Contributors: Sai Kishor Kothakota diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 8143d8f9c5..9e8010eeb9 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.8.0 + 4.9.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 2fdeef4860..74657cd12b 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-04-30) +------------------ * Deactivate the controllers when they return error similar to hardware (`#1499 `_) * Component parser: Get mimic information from URDF (`#1256 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 7de94fc053..884ffa9384 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.8.0 + 4.9.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index e3c9f7216c..6491f5e292 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-04-30) +------------------ 4.8.0 (2024-03-27) ------------------ diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index db64f0c28d..32264bd09e 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.8.0 + 4.9.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 3364794961..6e1528bc15 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-04-30) +------------------ * Add missing calculate_dynamics (`#1498 `_) * Component parser: Get mimic information from URDF (`#1256 `_) * Contributors: Christoph Fröhlich diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 41daac50cd..5274726257 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.8.0 + 4.9.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 3b657792e0..13161cdd4c 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-04-30) +------------------ * Component parser: Get mimic information from URDF (`#1256 `_) * Contributors: Christoph Fröhlich diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 49968cfd56..df6a035c6b 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.8.0 + 4.9.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 2a5ec53618..2bf49a13ec 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-04-30) +------------------ 4.8.0 (2024-03-27) ------------------ diff --git a/joint_limits/package.xml b/joint_limits/package.xml index a9a1d54dfd..e548387f48 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.8.0 + 4.9.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 23f78a8d34..aa93e8cc1b 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-04-30) +------------------ 4.8.0 (2024-03-27) ------------------ diff --git a/ros2_control/package.xml b/ros2_control/package.xml index e6716e478d..acb69b2069 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.8.0 + 4.9.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 63a4912237..b3ab824fea 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-04-30) +------------------ * Component parser: Get mimic information from URDF (`#1256 `_) * Contributors: Christoph Fröhlich diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index ecf8d57490..fe862846c4 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.8.0 + 4.9.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index f0b7b559f8..16946acdce 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-04-30) +------------------ * [CI] Specify runner/container images and codecov for joint_limits (`#1504 `_) * [CLI] Add `set_hardware_component_state` verb (`#1248 `_) * Contributors: Christoph Fröhlich diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index c8c9ae3dc7..fddcd2a0b4 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.8.0 + 4.9.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 441b7ea601..19fb09df99 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.8.0", + version="4.9.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 533c88149d..1691c2c8d5 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-04-30) +------------------ 4.8.0 (2024-03-27) ------------------ diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 61b92653a9..3c0e5ce652 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.8.0 + 4.9.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index c314d30353..107981322a 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.8.0", + version="4.9.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index dd01f8140b..94e13cd793 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.9.0 (2024-04-30) +------------------ * rosdoc2 for transmission_interface (`#1496 `_) * Component parser: Get mimic information from URDF (`#1256 `_) * Contributors: Christoph Fröhlich diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 2ca9d43928..36bbbd2a38 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.8.0 + 4.9.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From 5d0d4966e52099acb6ebaf10c8ed50d44407bb5a Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Wed, 1 May 2024 12:37:27 +0200 Subject: [PATCH 3/8] Bump version of pre-commit hooks and run only once per month (#1479) * Bump version of pre-commit hooks * Run update only once per month --------- Co-authored-by: bmagyar <3524577+bmagyar@users.noreply.github.com> Co-authored-by: Christoph Froehlich --- .github/workflows/update-pre-commit.yml | 2 +- .pre-commit-config.yaml | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/update-pre-commit.yml b/.github/workflows/update-pre-commit.yml index 8b9545dff1..6bedaa0c97 100644 --- a/.github/workflows/update-pre-commit.yml +++ b/.github/workflows/update-pre-commit.yml @@ -5,7 +5,7 @@ name: Auto Update pre-commit on: workflow_dispatch: schedule: - - cron: '0 0 * * 0' # Run every Sunday at midnight + - cron: '0 0 1 * *' # Runs at 00:00, on day 1 of the month jobs: auto_update_and_create_pr: diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 19f07a8e77..e039dd7ebb 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -49,7 +49,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.3.0 + rev: 24.4.2 hooks: - id: black args: ["--line-length=99"] @@ -62,7 +62,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.3 + rev: v18.1.4 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -132,7 +132,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.28.1 + rev: 0.28.2 hooks: - id: check-github-workflows args: ["--verbose"] From c935b502733c68bc7879bcfd45fb9fd471ad0259 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 3 May 2024 22:15:14 +0200 Subject: [PATCH 4/8] Add controller exception handling in controller manager (#1507) --- controller_manager/src/controller_manager.cpp | 182 +++++++++++++++--- 1 file changed, 150 insertions(+), 32 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e3729b90f8..43fdf5d015 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -562,11 +562,26 @@ controller_interface::return_type ControllerManager::unload_controller( get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? - const auto new_state = controller.c->get_node()->cleanup(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + try { - RCLCPP_WARN( - get_logger(), "Failed to clean-up the controller '%s' before unloading!", + const auto new_state = controller.c->get_node()->cleanup(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + RCLCPP_WARN( + get_logger(), "Failed to clean-up the controller '%s' before unloading!", + controller_name.c_str()); + } + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Failed to clean-up the controller '%s' before unloading: %s", + controller_name.c_str(), e.what()); + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Failed to clean-up the controller '%s' before unloading", controller_name.c_str()); } } @@ -631,22 +646,49 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for // cleaning-up controllers? - new_state = controller->get_node()->cleanup(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + try + { + new_state = controller->get_node()->cleanup(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + RCLCPP_ERROR( + get_logger(), "Controller '%s' can not be cleaned-up before configuring", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (...) { RCLCPP_ERROR( - get_logger(), "Controller '%s' can not be cleaned-up before configuring", + get_logger(), "Caught exception while cleaning-up controller '%s' before configuring", controller_name.c_str()); return controller_interface::return_type::ERROR; } } - new_state = controller->configure(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + try + { + new_state = controller->configure(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + RCLCPP_ERROR( + get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.", + controller_name.c_str(), new_state.label().c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), "After configuring, controller '%s' is in state '%s' , expected inactive.", - controller_name.c_str(), new_state.label().c_str()); + get_logger(), "Caught exception while configuring controller '%s': %s", + controller_name.c_str(), e.what()); + return controller_interface::return_type::ERROR; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while configuring controller '%s'", + controller_name.c_str()); return controller_interface::return_type::ERROR; } @@ -1286,14 +1328,35 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co } const rclcpp::NodeOptions controller_node_options = determine_controller_node_options(controller); - if ( - controller.c->init( - controller.info.name, robot_description_, get_update_rate(), get_namespace(), - controller_node_options) == controller_interface::return_type::ERROR) + // Catch whatever exception the controller might throw + try + { + if ( + controller.c->init( + controller.info.name, robot_description_, get_update_rate(), get_namespace(), + controller_node_options) == controller_interface::return_type::ERROR) + { + to.clear(); + RCLCPP_ERROR( + get_logger(), "Could not initialize the controller named '%s'", + controller.info.name.c_str()); + return nullptr; + } + } + catch (const std::exception & e) { to.clear(); RCLCPP_ERROR( - get_logger(), "Could not initialize the controller named '%s'", controller.info.name.c_str()); + get_logger(), "Caught exception while initializing controller '%s': %s", + controller.info.name.c_str(), e.what()); + return nullptr; + } + catch (...) + { + to.clear(); + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while initializing controller '%s'", + controller.info.name.c_str()); return nullptr; } @@ -1336,18 +1399,37 @@ void ControllerManager::deactivate_controllers( auto controller = found_it->c; if (is_controller_active(*controller)) { - const auto new_state = controller->get_node()->deactivate(); - controller->release_interfaces(); - // if it is a chainable controller, make the reference interfaces unavailable on deactivation - if (controller->is_chainable()) + try + { + const auto new_state = controller->get_node()->deactivate(); + controller->release_interfaces(); + + // if it is a chainable controller, make the reference interfaces unavailable on + // deactivation + if (controller->is_chainable()) + { + resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); + } + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + RCLCPP_ERROR( + get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive", + controller_name.c_str(), new_state.label().c_str()); + } + } + catch (const std::exception & e) { - resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); + RCLCPP_ERROR( + get_logger(), "Caught exception while deactivating the controller '%s': %s", + controller_name.c_str(), e.what()); + continue; } - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + catch (...) { RCLCPP_ERROR( - get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive", - controller_name.c_str(), new_state.label().c_str()); + get_logger(), "Caught unknown exception while deactivating the controller '%s'", + controller_name.c_str()); + continue; } } } @@ -1503,15 +1585,32 @@ void ControllerManager::activate_controllers( } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); - const auto new_state = controller->get_node()->activate(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + try + { + const auto new_state = controller->get_node()->activate(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + RCLCPP_ERROR( + get_logger(), + "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).", + controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(), + hardware_interface::lifecycle_state_names::ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + } + catch (const std::exception & e) { RCLCPP_ERROR( - get_logger(), - "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).", - controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(), - hardware_interface::lifecycle_state_names::ACTIVE, - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + get_logger(), "Caught exception while activating the controller '%s': %s", + controller_name.c_str(), e.what()); + continue; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while activating the controller '%s'", + controller_name.c_str()); + continue; } // if it is a chainable controller, make the reference interfaces available on activation @@ -2050,7 +2149,26 @@ controller_interface::return_type ControllerManager::update( { const auto controller_actual_period = (time - *loaded_controller.next_update_cycle_time) + controller_period; - auto controller_ret = loaded_controller.c->update(time, controller_actual_period); + auto controller_ret = controller_interface::return_type::OK; + // Catch exceptions thrown by the controller update function + try + { + controller_ret = loaded_controller.c->update(time, controller_actual_period); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Caught exception while updating controller '%s': %s", + loaded_controller.info.name.c_str(), e.what()); + controller_ret = controller_interface::return_type::ERROR; + } + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while updating controller '%s'", + loaded_controller.info.name.c_str()); + controller_ret = controller_interface::return_type::ERROR; + } if ( *loaded_controller.next_update_cycle_time == From 4d772b2e28c3f040920995c5a2136907de0f0541 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 4 May 2024 16:14:14 +0200 Subject: [PATCH 5/8] Add a functionality to look for the controller type in the params file when not parsed (#1502) --- controller_manager/CMakeLists.txt | 2 + .../controller_manager/launch_utils.py | 7 +++ .../controller_manager/spawner.py | 32 +++++++++++- .../test_controller_spawner_with_type.yaml | 13 +++++ .../test/test_spawner_unspawner.cpp | 51 +++++++++++++++++++ 5 files changed, 103 insertions(+), 2 deletions(-) create mode 100644 controller_manager/test/test_controller_spawner_with_type.yaml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index e267856eb1..60e121e3f2 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -189,6 +189,8 @@ if(BUILD_TESTING) test_controller ros2_control_test_assets::ros2_control_test_assets ) + install(FILES test/test_controller_spawner_with_type.yaml + DESTINATION test) ament_add_gmock(test_hardware_management_srvs test/test_hardware_management_srvs.cpp diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 5a23c02cec..c7c5974a94 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import warnings from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PythonExpression @@ -61,6 +62,12 @@ def generate_load_controller_launch_description( ] if controller_type: + warnings.warn( + "The 'controller_type' argument is deprecated and will be removed in future releases." + " Declare the controller type parameter in the param file instead.", + DeprecationWarning, + stacklevel=2, + ) spawner_arguments += ["--controller-type", controller_type] if controller_params_file: diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 1d11d80fe1..4f52a26e0b 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,6 +19,7 @@ import sys import time import warnings +import yaml from controller_manager import ( configure_controller, @@ -135,6 +136,21 @@ def is_controller_loaded(node, controller_manager, controller_name): return any(c.name == controller_name for c in controllers) +def get_parameter_from_param_file(controller_name, parameter_file, parameter_name): + with open(parameter_file) as f: + parameters = yaml.safe_load(f) + if controller_name in parameters: + value = parameters[controller_name] + if not isinstance(value, dict) or "ros__parameters" not in value: + raise RuntimeError( + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {controller_name}" + ) + if parameter_name in parameters[controller_name]["ros__parameters"]: + return parameters[controller_name]["ros__parameters"][parameter_name] + else: + return None + + def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() @@ -170,7 +186,7 @@ def main(args=None): parser.add_argument( "-t", "--controller-type", - help="If not provided it should exist in the controller manager namespace", + help="If not provided it should exist in the controller manager namespace (deprecated)", default=None, required=False, ) @@ -201,9 +217,16 @@ def main(args=None): controller_manager_name = args.controller_manager controller_namespace = args.namespace param_file = args.param_file - controller_type = args.controller_type controller_manager_timeout = args.controller_manager_timeout + if args.controller_type: + warnings.filterwarnings("always") + warnings.warn( + "The '--controller-type' argument is deprecated and will be removed in future releases." + " Declare the controller type parameter in the param file instead.", + DeprecationWarning, + ) + if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) @@ -226,6 +249,7 @@ def main(args=None): return 1 for controller_name in controller_names: + controller_type = args.controller_type prefixed_controller_name = controller_name if controller_namespace: prefixed_controller_name = controller_namespace + "/" + controller_name @@ -237,6 +261,10 @@ def main(args=None): + bcolors.ENDC ) else: + if not controller_type and param_file: + controller_type = get_parameter_from_param_file( + controller_name, param_file, "type" + ) if controller_type: parameter = Parameter() parameter.name = prefixed_controller_name + ".type" diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml new file mode 100644 index 0000000000..95a6efba30 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -0,0 +1,13 @@ +ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint0"] + +chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + +ctrl_with_parameters_and_no_type: + ros__parameters: + joint_names: ["joint2"] diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 3b774035aa..0030d9d148 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -23,6 +23,7 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "test_chainable_controller/test_chainable_controller.hpp" #include "test_controller/test_controller.hpp" using ::testing::_; @@ -259,6 +260,56 @@ TEST_F(TestLoadController, spawner_test_type_in_arg) ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } +TEST_F(TestLoadController, spawner_test_type_in_params_file) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.type, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.c->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path), + 256); + // Will still be same as the current call will fail + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} + TEST_F(TestLoadController, unload_on_kill) { // Launch spawner with unload on kill From 01120832a675e2750c1a41798e41f34916cdc1d5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 4 May 2024 16:18:37 +0200 Subject: [PATCH 6/8] Add more common hardware interface type constants (#1500) --- .../types/hardware_interface_type_values.hpp | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp index 27b5207a0f..f6f6e052e3 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp @@ -25,6 +25,28 @@ constexpr char HW_IF_VELOCITY[] = "velocity"; constexpr char HW_IF_ACCELERATION[] = "acceleration"; /// Constant defining effort interface name constexpr char HW_IF_EFFORT[] = "effort"; +/// Constant defining torque interface name +constexpr char HW_IF_TORQUE[] = "torque"; +/// Constant defining force interface name +constexpr char HW_IF_FORCE[] = "force"; +/// Constant defining current interface name +constexpr char HW_IF_CURRENT[] = "current"; +/// Constant defining temperature interface name +constexpr char HW_IF_TEMPERATURE[] = "temperature"; + +/// Gains interface constants +/// Constant defining proportional gain interface name +constexpr char HW_IF_PROPORTIONAL_GAIN[] = "proportional"; +/// Constant defining integral gain interface name +constexpr char HW_IF_INTEGRAL_GAIN[] = "integral"; +/// Constant defining derivative gain interface name +constexpr char HW_IF_DERIVATIVE_GAIN[] = "derivative"; +/// Constant defining integral clamp interface name +constexpr char HW_IF_INTEGRAL_CLAMP_MAX[] = "integral_clamp_max"; +/// Constant defining integral clamp interface name +constexpr char HW_IF_INTEGRAL_CLAMP_MIN[] = "integral_clamp_min"; +/// Constant defining the feedforward interface name +constexpr char HW_IF_FEEDFORWARD[] = "feedforward"; } // namespace hardware_interface #endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_ From 7fdf472c56ac809a6c925f763b299ec66bb3aae0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 4 May 2024 19:44:25 +0200 Subject: [PATCH 7/8] Add fallback controllers list to the ControllerInfo (#1503) --- controller_manager/CMakeLists.txt | 4 ++ .../controller_manager/spawner.py | 46 ++++++++++++++++ controller_manager/src/controller_manager.cpp | 11 ++++ ...ler_spawner_with_fallback_controllers.yaml | 13 +++++ .../test/test_spawner_unspawner.cpp | 55 +++++++++++++++++++ .../hardware_interface/controller_info.hpp | 3 + 6 files changed, 132 insertions(+) create mode 100644 controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 60e121e3f2..d99a7f7330 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -189,6 +189,10 @@ if(BUILD_TESTING) test_controller ros2_control_test_assets::ros2_control_test_assets ) + + install(FILES test/test_controller_spawner_with_fallback_controllers.yaml + DESTINATION test) + install(FILES test/test_controller_spawner_with_type.yaml DESTINATION test) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 4f52a26e0b..dbefd360b9 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -210,6 +210,14 @@ def main(args=None): action="store_true", required=False, ) + parser.add_argument( + "--fallback_controllers", + help="Fallback controllers list are activated as a fallback strategy when the" + " spawned controllers fail. When the argument is provided, it takes precedence over" + " the fallback_controllers list in the param file", + default=None, + nargs="+", + ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) @@ -249,6 +257,7 @@ def main(args=None): return 1 for controller_name in controller_names: + fallback_controllers = args.fallback_controllers controller_type = args.controller_type prefixed_controller_name = controller_name if controller_namespace: @@ -329,6 +338,43 @@ def main(args=None): ) return 1 + if not fallback_controllers and param_file: + fallback_controllers = get_parameter_from_param_file( + controller_name, param_file, "fallback_controllers" + ) + + if fallback_controllers: + parameter = Parameter() + parameter.name = prefixed_controller_name + ".fallback_controllers" + parameter.value = get_parameter_value(string_value=str(fallback_controllers)) + + response = call_set_parameters( + node=node, node_name=controller_manager_name, parameters=[parameter] + ) + assert len(response.results) == 1 + result = response.results[0] + if result.successful: + node.get_logger().info( + bcolors.OKCYAN + + 'Setting fallback_controllers to ["' + + ",".join(fallback_controllers) + + '"] for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + else: + node.get_logger().fatal( + bcolors.FAIL + + 'Could not set fallback_controllers to ["' + + ",".join(fallback_controllers) + + '"] for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + return 1 + ret = load_controller(node, controller_manager_name, controller_name) if not ret.ok: node.get_logger().fatal( diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 43fdf5d015..6cb7b025b1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -482,6 +482,17 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.parameters_file = parameters_file; } + const std::string fallback_ctrl_param = controller_name + ".fallback_controllers"; + std::vector fallback_controllers; + if (!has_parameter(fallback_ctrl_param)) + { + declare_parameter(fallback_ctrl_param, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); + } + if (get_parameter(fallback_ctrl_param, fallback_controllers) && !fallback_controllers.empty()) + { + controller_spec.info.fallback_controllers_names = fallback_controllers; + } + return add_controller_impl(controller_spec); } diff --git a/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml b/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml new file mode 100644 index 0000000000..bc93f162c1 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_with_fallback_controllers.yaml @@ -0,0 +1,13 @@ +ctrl_1: + ros__parameters: + joint_names: ["joint1"] + +ctrl_2: + ros__parameters: + joint_names: ["joint2"] + fallback_controllers: ["ctrl_6", "ctrl_7", "ctrl_8"] + +ctrl_3: + ros__parameters: + joint_names: ["joint3"] + fallback_controllers: ["ctrl_9"] diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 0030d9d148..d65689c6f2 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -324,3 +324,58 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } + +TEST_F(TestLoadController, spawner_test_fallback_controllers) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; + + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 " + "-p " + + test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT( + ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + } + + // Try to spawn now the controller with fallback controllers inside the yaml + EXPECT_EQ( + call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT( + ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); + ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT( + ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); + ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto ctrl_3 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); + ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); + ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + } +} diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 61a51a134a..a38fb99cb3 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -38,6 +38,9 @@ struct ControllerInfo /// List of claimed interfaces by the controller. std::vector claimed_interfaces; + + /// List of fallback controller names to be activated if this controller fails. + std::vector fallback_controllers_names; }; } // namespace hardware_interface From 607755e89636efdd13f6d70ebc07376f9020a820 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 7 May 2024 15:08:40 +0200 Subject: [PATCH 8/8] Parse URDF joint hard limits into the HardwareInfo structure (#1472) --- hardware_interface/CMakeLists.txt | 1 + .../hardware_interface/hardware_info.hpp | 8 + hardware_interface/package.xml | 1 + hardware_interface/src/component_parser.cpp | 215 ++++++- .../test/test_component_parser.cpp | 539 ++++++++++++++++++ .../components_urdfs.hpp | 175 ++++++ .../ros2_control_test_assets/descriptions.hpp | 371 +++++++++++- 7 files changed, 1300 insertions(+), 10 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 4ea7fdc2f7..a5debef3b7 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rcutils TinyXML2 tinyxml2_vendor + joint_limits urdf ) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 97abc00438..cb548b9bf4 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -19,6 +19,8 @@ #include #include +#include "joint_limits/joint_limits.hpp" + namespace hardware_interface { /** @@ -42,6 +44,8 @@ struct InterfaceInfo std::string data_type; /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. int size; + /// (Optional) enable or disable the limits for the command interfaces + bool enable_limits; }; /// @brief This structure stores information about a joint that is mimicking another joint @@ -163,6 +167,10 @@ struct HardwareInfo * The XML contents prior to parsing */ std::string original_xml; + /** + * The URDF parsed limits of the hardware components joint command interfaces + */ + std::unordered_map limits; }; } // namespace hardware_interface diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 5274726257..b79d280a11 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -16,6 +16,7 @@ rclcpp_lifecycle rcpputils tinyxml2_vendor + joint_limits urdf rcutils diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 0841265e81..aad1d827ec 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -26,6 +26,8 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limits_urdf.hpp" namespace { @@ -43,6 +45,8 @@ constexpr const auto kCommandInterfaceTag = "command_interface"; constexpr const auto kStateInterfaceTag = "state_interface"; constexpr const auto kMinTag = "min"; constexpr const auto kMaxTag = "max"; +constexpr const auto kLimitsTag = "limits"; +constexpr const auto kEnableAttribute = "enable"; constexpr const auto kInitialValueTag = "initial_value"; constexpr const auto kMimicAttribute = "mimic"; constexpr const auto kDataTypeAttribute = "data_type"; @@ -289,6 +293,15 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( interface.max = interface_param->second; } + // Option enable or disable the interface limits, by default they are enabled + interface.enable_limits = true; + const auto * limits_it = interfaces_it->FirstChildElement(kLimitsTag); + if (limits_it) + { + interface.enable_limits = + parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name())); + } + // Optional initial_value attribute interface_param = interface_params.find(kInitialValueTag); if (interface_param != interface_params.end()) @@ -333,11 +346,21 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it } } + // Option enable or disable the interface limits, by default they are enabled + bool enable_limits = true; + const auto * limits_it = component_it->FirstChildElement(kLimitsTag); + if (limits_it) + { + enable_limits = parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name())); + } + // Parse all command interfaces const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag); while (command_interfaces_it) { - component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it)); + InterfaceInfo cmd_info = parse_interfaces_from_xml(command_interfaces_it); + cmd_info.enable_limits &= enable_limits; + component.command_interfaces.push_back(cmd_info); command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag); } @@ -345,7 +368,9 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it const auto * state_interfaces_it = component_it->FirstChildElement(kStateInterfaceTag); while (state_interfaces_it) { - component.state_interfaces.push_back(parse_interfaces_from_xml(state_interfaces_it)); + InterfaceInfo state_info = parse_interfaces_from_xml(state_interfaces_it); + state_info.enable_limits &= enable_limits; + component.state_interfaces.push_back(state_info); state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag); } @@ -589,6 +614,171 @@ HardwareInfo parse_resource_from_xml( return hardware; } +/** + * @brief Retrieve the min and max values from the interface tag. + * @param itf The interface tag to retrieve the values from. + * @param min The minimum value to be retrieved. + * @param max The maximum value to be retrieved. + * @return true if the values are retrieved, false otherwise. + */ +bool retrieve_min_max_interface_values(const InterfaceInfo & itf, double & min, double & max) +{ + try + { + if (itf.min.empty() && itf.max.empty()) + { + // If the limits don't exist then return false as they are not retrieved + return false; + } + if (!itf.min.empty()) + { + min = hardware_interface::stod(itf.min); + } + if (!itf.max.empty()) + { + max = hardware_interface::stod(itf.max); + } + return true; + } + catch (const std::invalid_argument & err) + { + std::cerr << "Error parsing the limits for the interface: " << itf.name << " from the tags [" + << kMinTag << ": '" << itf.min << "' and " << kMaxTag << ": '" << itf.max + << "'] within " << kROS2ControlTag << " tag inside the URDF. Skipping it" + << std::endl; + return false; + } +} + +/** + * @brief Set custom values for acceleration and jerk limits (no URDF standard) + * @param itr The interface tag to retrieve the values from. + * @param limits The joint limits to be set. + */ +void set_custom_interface_values(const InterfaceInfo & itr, joint_limits::JointLimits & limits) +{ + if (itr.name == hardware_interface::HW_IF_ACCELERATION) + { + double max_decel(std::numeric_limits::quiet_NaN()), + max_accel(std::numeric_limits::quiet_NaN()); + if (detail::retrieve_min_max_interface_values(itr, max_decel, max_accel)) + { + if (std::isfinite(max_decel)) + { + limits.max_deceleration = std::fabs(max_decel); + if (!std::isfinite(max_accel)) + { + limits.max_acceleration = std::fabs(limits.max_deceleration); + } + limits.has_deceleration_limits = itr.enable_limits; + } + if (std::isfinite(max_accel)) + { + limits.max_acceleration = max_accel; + + if (!std::isfinite(limits.max_deceleration)) + { + limits.max_deceleration = std::fabs(limits.max_acceleration); + } + limits.has_acceleration_limits = itr.enable_limits; + } + } + } + else if (itr.name == "jerk") + { + if (!itr.min.empty()) + { + std::cerr << "Error parsing the limits for the interface: " << itr.name + << " from the tag: " << kMinTag << " within " << kROS2ControlTag + << " tag inside the URDF. Jerk only accepts max limits." << std::endl; + } + double min_jerk(std::numeric_limits::quiet_NaN()), + max_jerk(std::numeric_limits::quiet_NaN()); + if ( + !itr.max.empty() && detail::retrieve_min_max_interface_values(itr, min_jerk, max_jerk) && + std::isfinite(max_jerk)) + { + limits.max_jerk = std::abs(max_jerk); + limits.has_jerk_limits = itr.enable_limits; + } + } + else + { + if (!itr.min.empty() || !itr.max.empty()) + { + std::cerr << "Unable to parse the limits for the interface: " << itr.name + << " from the tags [" << kMinTag << " and " << kMaxTag << "] within " + << kROS2ControlTag + << " tag inside the URDF. Supported interfaces for joint limits are: " + "position, velocity, effort, acceleration and jerk." + << std::endl; + } + } +} + +/** + * @brief Retrieve the limits from ros2_control command interface tags and override URDF limits if + * restrictive + * @param interfaces The interfaces to retrieve the limits from. + * @param limits The joint limits to be set. + */ +void update_interface_limits( + const std::vector & interfaces, joint_limits::JointLimits & limits) +{ + for (auto & itr : interfaces) + { + if (itr.name == hardware_interface::HW_IF_POSITION) + { + limits.min_position = limits.has_position_limits && itr.enable_limits + ? limits.min_position + : -std::numeric_limits::max(); + limits.max_position = limits.has_position_limits && itr.enable_limits + ? limits.max_position + : std::numeric_limits::max(); + double min_pos(limits.min_position), max_pos(limits.max_position); + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_pos, max_pos)) + { + limits.min_position = std::max(min_pos, limits.min_position); + limits.max_position = std::min(max_pos, limits.max_position); + limits.has_position_limits = true; + } + limits.has_position_limits &= itr.enable_limits; + } + else if (itr.name == hardware_interface::HW_IF_VELOCITY) + { + limits.max_velocity = + limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits::max(); + // Apply the most restrictive one in the case + double min_vel(-limits.max_velocity), max_vel(limits.max_velocity); + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_vel, max_vel)) + { + max_vel = std::min(std::abs(min_vel), max_vel); + limits.max_velocity = std::min(max_vel, limits.max_velocity); + limits.has_velocity_limits = true; + } + limits.has_velocity_limits &= itr.enable_limits; + } + else if (itr.name == hardware_interface::HW_IF_EFFORT) + { + limits.max_effort = + limits.has_effort_limits ? limits.max_effort : std::numeric_limits::max(); + // Apply the most restrictive one in the case + double min_eff(-limits.max_effort), max_eff(limits.max_effort); + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_eff, max_eff)) + { + max_eff = std::min(std::abs(min_eff), max_eff); + limits.max_effort = std::min(max_eff, limits.max_effort); + limits.has_effort_limits = true; + } + limits.has_effort_limits &= itr.enable_limits; + } + else + { + detail::set_custom_interface_values(itr, limits); + } + } +} + } // namespace detail std::vector parse_control_resources_from_urdf(const std::string & urdf) @@ -716,6 +906,27 @@ std::vector parse_control_resources_from_urdf(const std::string & hw_info.mimic_joints.push_back(mimic_joint); } } + // TODO(christophfroehlich) remove this code if deprecated mimic attribute - branch + // from above is removed (double check here, but throws already above if not found in URDF) + auto urdf_joint = model.getJoint(joint.name); + if (!urdf_joint) + { + std::cerr << "Joint: '" + joint.name + "' not found in URDF. Skipping limits parsing!" + << std::endl; + continue; + } + if (urdf_joint->type == urdf::Joint::FIXED) + { + throw std::runtime_error( + "Joint '" + joint.name + + "' is of type 'fixed'. " + "Fixed joints do not make sense in ros2_control."); + } + joint_limits::JointLimits limits; + getJointLimits(urdf_joint, limits); + // Take the most restricted one. Also valid for continuous-joint type only + detail::update_interface_limits(joint.command_interfaces, limits); + hw_info.limits[joint.name] = limits; } } diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 968d41ed97..946fda7c15 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -32,6 +32,7 @@ class TestComponentParser : public Test void SetUp() override {} }; +using hardware_interface::HW_IF_ACCELERATION; using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; @@ -137,6 +138,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1"); ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface) @@ -174,6 +190,22 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3)); EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, HW_IF_EFFORT); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sensor) @@ -218,6 +250,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("lower_limits"), "-100"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("upper_limits"), "100"); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor) @@ -248,6 +295,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte ASSERT_THAT(hardware_info.sensors, SizeIs(0)); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } + hardware_info = control_hardware.at(1); EXPECT_EQ(hardware_info.name, "RRBotForceTorqueSensor2D"); @@ -259,6 +321,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte EXPECT_EQ(hardware_info.sensors[0].name, "tcp_fts_sensor"); EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp"); + ASSERT_THAT(hardware_info.limits, SizeIs(0)); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot) @@ -281,6 +344,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } hardware_info = control_hardware.at(1); @@ -294,6 +371,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint2"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot_with_sensors) @@ -328,6 +419,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot hardware_info.transmissions[0].joints[0].mechanical_reduction, DoubleNear(1024.0 / M_PI, 0.01)); ASSERT_THAT(hardware_info.transmissions[0].actuators, SizeIs(1)); EXPECT_THAT(hardware_info.transmissions[0].actuators[0].name, "actuator1"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } hardware_info = control_hardware.at(1); @@ -345,6 +449,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1"); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } hardware_info = control_hardware.at(2); @@ -362,6 +479,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty()); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } hardware_info = control_hardware.at(3); @@ -379,6 +509,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty()); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_transmission) @@ -451,6 +594,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) EXPECT_EQ(hardware_info.sensors[1].type, "sensor"); ASSERT_THAT(hardware_info.sensors[1].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.sensors[1].state_interfaces[0].name, "image"); + // There will be no limits as the ros2_control tag has only sensor info + ASSERT_THAT(hardware_info.limits, SizeIs(0)); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) @@ -500,6 +645,17 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) EXPECT_THAT(actuator.offset, DoubleEq(0.0)); ASSERT_THAT(transmission.parameters, SizeIs(1)); EXPECT_EQ(transmission.parameters.at("additional_special_parameter"), "1337"); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); } TEST_F(TestComponentParser, successfully_parse_locale_independent_double) @@ -568,6 +724,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_EQ(hardware_info.gpios[1].command_interfaces[0].name, "vacuum"); EXPECT_THAT(hardware_info.transmissions, IsEmpty()); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + // Position limits are limited in the ros2_control tag + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_data_type) @@ -616,6 +786,362 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_interfaces) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets:: + valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "RRBotSystemWithGPIO"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + + ASSERT_THAT(hardware_info.joints, SizeIs(3)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(5)); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].size, 1); + EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].size, 1); + + EXPECT_EQ(hardware_info.joints[1].name, "joint2"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(5)); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].size, 1); + EXPECT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].size, 1); + + EXPECT_EQ(hardware_info.joints[2].name, "joint3"); + EXPECT_EQ(hardware_info.joints[2].type, "joint"); + EXPECT_THAT(hardware_info.joints[2].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].size, 1); + + ASSERT_THAT(hardware_info.gpios, SizeIs(1)); + + EXPECT_EQ(hardware_info.gpios[0].name, "flange_IOS"); + EXPECT_EQ(hardware_info.gpios[0].type, "gpio"); + EXPECT_THAT(hardware_info.gpios[0].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].name, "digital_output"); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].data_type, "bool"); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].size, 2); + EXPECT_THAT(hardware_info.gpios[0].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].name, "analog_input"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].size, 3); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].name, "image"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].data_type, "cv::Mat"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); + + EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT( + hardware_info.limits.at("joint1").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint1").min_position, + DoubleNear(-std::numeric_limits::max(), 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); + + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_THAT( + hardware_info.limits.at("joint2").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint2").min_position, + DoubleNear(-std::numeric_limits::max(), 1e-5)); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5)); + + EXPECT_TRUE(hardware_info.limits.at("joint3").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_acceleration, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5)); +} + +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_unavailable_interfaces) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_unavailable_interfaces + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "RRBotSystemWithGPIO"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + + ASSERT_THAT(hardware_info.joints, SizeIs(3)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(5)); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].size, 1); + EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].size, 1); + + EXPECT_EQ(hardware_info.joints[1].name, "joint2"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(5)); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].size, 1); + EXPECT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].size, 1); + + EXPECT_EQ(hardware_info.joints[2].name, "joint3"); + EXPECT_EQ(hardware_info.joints[2].type, "joint"); + EXPECT_THAT(hardware_info.joints[2].command_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].size, 1); + + ASSERT_THAT(hardware_info.gpios, SizeIs(0)); + + EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT( + hardware_info.limits.at("joint1").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint1").min_position, + DoubleNear(-std::numeric_limits::max(), 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); + + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_THAT( + hardware_info.limits.at("joint2").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint2").min_position, + DoubleNear(-std::numeric_limits::max(), 1e-5)); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5)); + + EXPECT_TRUE(hardware_info.limits.at("joint3").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_acceleration, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5)); +} + +TEST_F(TestComponentParser, throw_on_parse_invalid_urdf_system_missing_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_revolute_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); + + urdf_to_test = std::string(ros2_control_test_assets::urdf_head_prismatic_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, throw_on_parse_urdf_system_with_command_fixed_joint) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::invalid_urdf_ros2_control_system_with_command_fixed_joint + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_missing_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_all_interfaces + + ros2_control_test_assets::urdf_tail; + EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test)); + + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + ASSERT_GT(hardware_info.limits.count("joint1"), 0); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)) + << "velocity constraint from ros2_control_tag"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.2, 1e-5)) + << "effort constraint from ros2_control_tag"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); + + ASSERT_GT(hardware_info.limits.count("joint2"), 0); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); +} + +TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_with_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_all_interfaces + + ros2_control_test_assets::urdf_tail; + EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test)); + + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + ASSERT_GT(hardware_info.limits.count("joint1"), 0); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)) + << "velocity URDF constraint overridden by ros2_control tag"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)) + << "effort constraint from URDF"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); + + ASSERT_GT(hardware_info.limits.count("joint2"), 0); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_TRUE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5)) + << "velocity constraint from URDF"; + EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5)) + << "effort constraint from URDF"; + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); +} + TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = @@ -640,6 +1166,19 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), ""); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, negative_size_throws_error) diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 7b2dd46e7a..7b46eda9c0 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -400,6 +400,166 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type = )"; +// 12. Industrial Robots with integrated GPIO with few disabled limits in joints +const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + + -1 + 1 + + + -0.05 + 0.1 + + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + + + + + + + -1 + 1 + + + + + -0.3 + 0.3 + + + -2.0 + 2.0 + + + + + + 1.0 + + + + + + + + +)"; +const auto valid_urdf_ros2_control_system_robot_with_unavailable_interfaces = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + + -1 + 1 + + + -0.05 + 0.1 + + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + + + + + + + -1 + 1 + + + + + -0.3 + 0.3 + + + + + + + 1.0 + + + -0.3 + 0.3 + + + +)"; +const auto valid_urdf_ros2_control_system_robot_with_all_interfaces = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + -1 + 1 + + + -0.05 + 0.1 + + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + + + + + + + + + + + + +)"; + const auto valid_urdf_ros2_control_parameter_empty = R"( @@ -567,6 +727,21 @@ const auto invalid_urdf2_transmission_given_too_many_joints = )"; +const auto invalid_urdf_ros2_control_system_with_command_fixed_joint = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + + + + +)"; + } // namespace ros2_control_test_assets #endif // ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_ diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 308751e0d8..f5f668fd12 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -23,10 +23,6 @@ namespace ros2_control_test_assets const auto urdf_head = R"( - - - - @@ -149,13 +145,370 @@ const auto urdf_head = )"; + +const auto urdf_head_revolute_missing_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_head_continuous_missing_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_head_continuous_with_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_head_prismatic_missing_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + const auto urdf_head_mimic = R"( - - - - @@ -503,6 +856,7 @@ const auto diffbot_urdf = + @@ -538,6 +892,7 @@ const auto diffbot_urdf = +