From 20d49a14275775ac738a34600950c9ddd8f027d3 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 31 Jan 2024 21:04:19 +0000 Subject: [PATCH 1/6] Fix version --- hardware_interface_testing/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 841c77626f..4ad118a144 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 0.0.0 + 4.3.0 ros2_control hardware interface testing Bence Magyar Denis Štogl From 7166de7e2ea9b3c1d45f98bd3aa62efafc1c8e47 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 31 Jan 2024 21:06:55 +0000 Subject: [PATCH 2/6] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 6 ++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 6 ++++++ hardware_interface_testing/CHANGELOG.rst | 9 +++++++++ joint_limits/CHANGELOG.rst | 6 ++++++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 11 files changed, 48 insertions(+) create mode 100644 hardware_interface_testing/CHANGELOG.rst diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index d2265d1442..af5e373076 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-20) ------------------ * Issue 695: Changing 'namespace\_' variables to 'node_namespace' to make it more explicit (`#1239 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 3699d3ec94..76fcd6a723 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Move `test_components` to own package (`#1325 `_) +* Fix controller parameter loading issue in different cases (`#1293 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.3.0 (2024-01-20) ------------------ * [CM] Better readability and maintainability: rename variables, move code to more logical places 🔧 (`#1227 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 6ebfcfd842..57907e24b6 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.3.0 (2024-01-20) ------------------ diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 820c2c793f..fcc2a2eabf 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Move `test_components` to own package (`#1325 `_) +* Fix controller parameter loading issue in different cases (`#1293 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.3.0 (2024-01-20) ------------------ * [RM] Fix crash for missing urdf in resource manager (`#1301 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst new file mode 100644 index 0000000000..025a438c0c --- /dev/null +++ b/hardware_interface_testing/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hardware_interface_testing +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Fix version +* Move `test_components` to own package (`#1325 `_) +* Contributors: Bence Magyar, Christoph Fröhlich diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 1df9702430..b0b7457b79 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [Format] Correct formatting of JointLimits URDF file. (`#1339 `_) +* Add header to import limits from standard URDF definition (`#1298 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + 4.3.0 (2024-01-20) ------------------ diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 2f21f0bab4..849b48e6f4 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-20) ------------------ diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 3d29c094d1..03486ba23a 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-20) ------------------ * [ResourceManager] adds test for uninitialized hardware (`#1243 `_) diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 7ddf9c472a..1dc4cd0afc 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-20) ------------------ * [docs] Remove joint_state_controller (`#1263 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 2786604992..85513effa0 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.3.0 (2024-01-20) ------------------ * Fix rqt controller manager crash on ros2_control restart (`#1273 `_) diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 312c187b3e..d500e6fe41 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.3.0 (2024-01-20) ------------------ * Improve transmission tests (`#1238 `_) From 3933648579bfe2e5cfa7422df27123cbca6a6c24 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 31 Jan 2024 21:09:11 +0000 Subject: [PATCH 3/6] 4.4.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 af5e373076..c6471c90f6 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-31) +------------------ 4.3.0 (2024-01-20) ------------------ diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 4d1911123a..919fdf8d20 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.3.0 + 4.4.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 76fcd6a723..f5bc17f0f3 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-31) +------------------ * Move `test_components` to own package (`#1325 `_) * Fix controller parameter loading issue in different cases (`#1293 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/controller_manager/package.xml b/controller_manager/package.xml index e534e39268..e58ff84c3d 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.3.0 + 4.4.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 57907e24b6..36245deaad 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.4.0 (2024-01-31) +------------------ 4.3.0 (2024-01-20) ------------------ diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 5f8aedc8cf..1beaf47741 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.3.0 + 4.4.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 fcc2a2eabf..b5fb894e0c 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-31) +------------------ * Move `test_components` to own package (`#1325 `_) * Fix controller parameter loading issue in different cases (`#1293 `_) * Contributors: Christoph Fröhlich, Sai Kishor Kothakota diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 13db76cbe4..e3a343c8ae 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.3.0 + 4.4.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 025a438c0c..1ad077aa5c 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.4.0 (2024-01-31) +------------------ * Fix version * Move `test_components` to own package (`#1325 `_) * Contributors: Bence Magyar, Christoph Fröhlich diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 4ad118a144..de7d75b2f5 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.3.0 + 4.4.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index b0b7457b79..aa9d8db111 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-31) +------------------ * [Format] Correct formatting of JointLimits URDF file. (`#1339 `_) * Add header to import limits from standard URDF definition (`#1298 `_) * Contributors: Dr. Denis, Sai Kishor Kothakota diff --git a/joint_limits/package.xml b/joint_limits/package.xml index e1e72a602a..9deb33af7d 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.3.0 + 4.4.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 849b48e6f4..3a54f3249c 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-31) +------------------ 4.3.0 (2024-01-20) ------------------ diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 9c33c40d08..9e7c446b66 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.3.0 + 4.4.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 03486ba23a..9e2f966d0d 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.4.0 (2024-01-31) +------------------ 4.3.0 (2024-01-20) ------------------ diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 78e93658ab..c403246015 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.3.0 + 4.4.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 1dc4cd0afc..b659a5bf97 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-31) +------------------ 4.3.0 (2024-01-20) ------------------ diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 3b1b82de70..33f265a607 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.3.0 + 4.4.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index b3c870e6e0..02c9f108d3 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.3.0", + version="4.4.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 85513effa0..07724c98a3 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.4.0 (2024-01-31) +------------------ 4.3.0 (2024-01-20) ------------------ diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 1e7c0683e7..c280751db7 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.3.0 + 4.4.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 05d95df62b..f93c231cd9 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="4.3.0", + version="4.4.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 d500e6fe41..c63c29c3ab 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.4.0 (2024-01-31) +------------------ 4.3.0 (2024-01-20) ------------------ diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 55ad994599..894bbd9b45 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.3.0 + 4.4.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 db110770c165b0a499ed2a3bace9237f6c457fbf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 31 Jan 2024 22:19:20 +0100 Subject: [PATCH 4/6] A method to get node options to setup the controller node #api-breaking (#1169) --- controller_interface/CMakeLists.txt | 2 + .../controller_interface_base.hpp | 22 +++- .../src/controller_interface_base.cpp | 2 +- .../test_chainable_controller_interface.cpp | 30 ++++- .../test/test_controller_interface.cpp | 18 ++- .../test/test_controller_node_options.yaml | 6 + .../test/test_controller_with_options.cpp | 103 +++++++++++++++++- .../test/test_controller_with_options.hpp | 35 ++---- controller_manager/src/controller_manager.cpp | 2 +- .../test_controller_failed_init.cpp | 8 -- .../test_controller_failed_init.hpp | 16 ++- 11 files changed, 183 insertions(+), 61 deletions(-) create mode 100644 controller_interface/test/test_controller_node_options.yaml diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index d02c422451..19a501dc62 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -40,6 +40,8 @@ if(BUILD_TESTING) ) ament_add_gmock(test_controller_with_options test/test_controller_with_options.cpp) + install(FILES test/test_controller_node_options.yaml + DESTINATION test) target_link_libraries(test_controller_with_options controller_interface ) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 2e953ee932..6ce483d1ff 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -113,10 +113,9 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy void release_interfaces(); CONTROLLER_INTERFACE_PUBLIC - virtual return_type init( + return_type init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & node_namespace = "", - const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true)); + const std::string & node_namespace, const rclcpp::NodeOptions & node_options); /// Custom configure method to read additional parameters for controller-nodes /* @@ -159,6 +158,23 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC const std::string & get_robot_description() const; + /** + * Method used by the controller_manager for base NodeOptions to instantiate the Lifecycle node + * of the controller upon loading the controller. + * + * \note The controller_manager will modify these NodeOptions in case a params file is passed + * by the spawner to load the controller parameters or when controllers are loaded in simulation + * (see ros2_control#1311, ros2_controllers#698 , ros2_controllers#795,ros2_controllers#966 for + * more details) + * + * @returns NodeOptions required for the configuration of the controller lifecycle node + */ + CONTROLLER_INTERFACE_PUBLIC + virtual rclcpp::NodeOptions define_custom_node_options() const + { + return rclcpp::NodeOptions().enable_logger_service(true); + } + /// Declare and initialize a parameter with a type. /** * diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 560feff5f9..e48e1d21b3 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -28,10 +28,10 @@ return_type ControllerInterfaceBase::init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, const std::string & node_namespace, const rclcpp::NodeOptions & node_options) { + urdf_ = urdf; node_ = std::make_shared( controller_name, node_namespace, node_options, false); // disable LifecycleNode service interfaces - urdf_ = urdf; try { diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index d81d2bfdad..47487e019c 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -21,7 +21,10 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); EXPECT_TRUE(controller.is_chainable()); @@ -33,7 +36,10 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); @@ -50,7 +56,10 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correc TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); // expect empty return because storage is not resized @@ -64,7 +73,10 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix"); @@ -79,7 +91,10 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); @@ -126,7 +141,10 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); EXPECT_FALSE(controller.is_in_chained_mode()); diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 24e780fab3..3976b2a81e 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -38,7 +38,10 @@ TEST(TestableControllerInterface, init) ASSERT_THROW(controller.get_node(), std::runtime_error); // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 10.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 10.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); // update_rate is set to default 0 @@ -60,7 +63,10 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) TestableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 1.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options), + controller_interface::return_type::OK); // initialize executor to be able to get parameter update auto executor = @@ -122,8 +128,10 @@ TEST(TestableControllerInterfaceInitError, init_with_error) TestableControllerInterfaceInitError controller; // initialize, create node + const auto node_options = controller.define_custom_node_options(); ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 100.0), controller_interface::return_type::ERROR); + controller.init(TEST_CONTROLLER_NAME, "", 100.0, "", node_options), + controller_interface::return_type::ERROR); rclcpp::shutdown(); } @@ -137,8 +145,10 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure) TestableControllerInterfaceInitFailure controller; // initialize, create node + const auto node_options = controller.define_custom_node_options(); ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::ERROR); + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::ERROR); rclcpp::shutdown(); } diff --git a/controller_interface/test/test_controller_node_options.yaml b/controller_interface/test/test_controller_node_options.yaml new file mode 100644 index 0000000000..0c23c7f9d8 --- /dev/null +++ b/controller_interface/test/test_controller_node_options.yaml @@ -0,0 +1,6 @@ +controller_name: + ros__parameters: + parameter_list: + parameter1: 20.0 + parameter2: 23.14 + parameter3: -52.323 diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index 51169e945c..1e22239215 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -16,7 +16,7 @@ #include #include - +#include "ament_index_cpp/get_package_prefix.hpp" #include "rclcpp/rclcpp.hpp" class FriendControllerWithOptions : public controller_with_options::ControllerWithOptions @@ -42,11 +42,14 @@ TEST(ControllerWithOption, init_with_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ(controller.init("controller_name", "", 50.0), controller_interface::return_type::OK); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), + controller_interface::return_type::OK); // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); + EXPECT_TRUE(node_options.arguments().empty()); // checks that the parameters have been correctly processed EXPECT_EQ(controller.params.size(), 3u); EXPECT_EQ(controller.params["parameter1"], 1.); @@ -55,6 +58,98 @@ TEST(ControllerWithOption, init_with_overrides) rclcpp::shutdown(); } +TEST(ControllerWithOption, init_with_node_options_arguments_parameters) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + // creates the controller + FriendControllerWithOptions controller; + auto controller_node_options = controller.define_custom_node_options(); + controller_node_options.arguments( + {"--ros-args", "-p", "parameter_list.parameter1:=1.", "-p", "parameter_list.parameter2:=2.", + "-p", "parameter_list.parameter3:=3."}); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller_node_options), + controller_interface::return_type::OK); + // checks that the node options have been updated + const auto & node_options = controller.get_node()->get_node_options(); + EXPECT_TRUE(node_options.allow_undeclared_parameters()); + EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); + EXPECT_EQ(7lu, node_options.arguments().size()); + // checks that the parameters have been correctly processed + EXPECT_EQ(controller.params.size(), 3u); + EXPECT_EQ(controller.params["parameter1"], 1.); + EXPECT_EQ(controller.params["parameter2"], 2.); + EXPECT_EQ(controller.params["parameter3"], 3.); + rclcpp::shutdown(); +} + +TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + // creates the controller + FriendControllerWithOptions controller; + const std::string params_file_path = ament_index_cpp::get_package_prefix("controller_interface") + + "/test/test_controller_node_options.yaml"; + std::cerr << params_file_path << std::endl; + auto controller_node_options = controller.define_custom_node_options(); + controller_node_options.arguments({"--ros-args", "--params-file", params_file_path}); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller_node_options), + controller_interface::return_type::OK); + // checks that the node options have been updated + const auto & node_options = controller.get_node()->get_node_options(); + EXPECT_TRUE(node_options.allow_undeclared_parameters()); + EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); + EXPECT_EQ(3lu, node_options.arguments().size()); + // checks that the parameters have been correctly processed + EXPECT_EQ(controller.params.size(), 3u); + EXPECT_EQ(controller.params["parameter1"], 20.0); + EXPECT_EQ(controller.params["parameter2"], 23.14); + EXPECT_EQ(controller.params["parameter3"], -52.323); + bool use_sim_time(true); + controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false); + ASSERT_FALSE(use_sim_time); + rclcpp::shutdown(); +} + +TEST( + ControllerWithOption, init_with_node_options_arguments_parameters_file_and_override_command_line) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + // creates the controller + FriendControllerWithOptions controller; + const std::string params_file_path = ament_index_cpp::get_package_prefix("controller_interface") + + "/test/test_controller_node_options.yaml"; + std::cerr << params_file_path << std::endl; + auto controller_node_options = controller.define_custom_node_options(); + controller_node_options.arguments( + {"--ros-args", "--params-file", params_file_path, "-p", "parameter_list.parameter1:=562.785", + "-p", "use_sim_time:=true"}); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller_node_options), + controller_interface::return_type::OK); + // checks that the node options have been updated + const auto & node_options = controller.get_node()->get_node_options(); + EXPECT_TRUE(node_options.allow_undeclared_parameters()); + EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); + EXPECT_EQ(7lu, node_options.arguments().size()); + // checks that the parameters have been correctly processed + EXPECT_EQ(controller.params.size(), 3u); + EXPECT_EQ(controller.params["parameter1"], 562.785); + EXPECT_EQ(controller.params["parameter2"], 23.14); + EXPECT_EQ(controller.params["parameter3"], -52.323); + bool use_sim_time(false); + controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false); + ASSERT_TRUE(use_sim_time); + rclcpp::shutdown(); +} + TEST(ControllerWithOption, init_without_overrides) { // mocks the declaration of overrides parameters in a yaml file @@ -63,7 +158,9 @@ TEST(ControllerWithOption, init_without_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ(controller.init("controller_name", "", 50.0), controller_interface::return_type::ERROR); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), + controller_interface::return_type::ERROR); // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index 1416a7d2df..fc909df361 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -36,36 +36,19 @@ class ControllerWithOptions : public controller_interface::ControllerInterface ControllerWithOptions() = default; LifecycleNodeInterface::CallbackReturn on_init() override { - return LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - controller_interface::return_type init( - const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & node_namespace = "", - const rclcpp::NodeOptions & node_options = - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)) override - { - ControllerInterface::init(controller_name, urdf, cm_update_rate, node_namespace, node_options); - - switch (on_init()) - { - case LifecycleNodeInterface::CallbackReturn::SUCCESS: - break; - case LifecycleNodeInterface::CallbackReturn::ERROR: - case LifecycleNodeInterface::CallbackReturn::FAILURE: - return controller_interface::return_type::ERROR; - } if (get_node()->get_parameters("parameter_list", params)) { RCLCPP_INFO_STREAM(get_node()->get_logger(), "I found " << params.size() << " parameters."); - return controller_interface::return_type::OK; - } - else - { - return controller_interface::return_type::ERROR; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } + return LifecycleNodeInterface::CallbackReturn::FAILURE; + } + + rclcpp::NodeOptions define_custom_node_options() const override + { + return rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); } controller_interface::InterfaceConfiguration command_interface_configuration() const override diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ab57c5a196..9b9b0dcf8f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2606,7 +2606,7 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( auto check_for_element = [](const auto & list, const auto & element) { return std::find(list.begin(), list.end(), element) != list.end(); }; - rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true); + rclcpp::NodeOptions controller_node_options = controller.c->define_custom_node_options(); std::vector node_options_arguments = controller_node_options.arguments(); const std::string ros_args_arg = "--ros-args"; if (controller.info.parameters_file.has_value()) diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp index a106d5cbdf..1b2276bf3c 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp @@ -31,14 +31,6 @@ TestControllerFailedInit::on_init() return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } -controller_interface::return_type TestControllerFailedInit::init( - const std::string & /* controller_name */, const std::string & /* urdf */, - unsigned int /*cm_update_rate*/, const std::string & /*node_namespace*/, - const rclcpp::NodeOptions & /*node_options*/) -{ - return controller_interface::return_type::ERROR; -} - controller_interface::return_type TestControllerFailedInit::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp index 315b0745b0..3f24df4e29 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp @@ -38,21 +38,19 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac CONTROLLER_MANAGER_PUBLIC virtual ~TestControllerFailedInit() = default; - CONTROLLER_INTERFACE_PUBLIC - controller_interface::return_type init( - const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & node_namespace = "", - const rclcpp::NodeOptions & node_options = - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)) override; - controller_interface::InterfaceConfiguration command_interface_configuration() const override { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } + rclcpp::NodeOptions define_custom_node_options() const override + { + return rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); + } + controller_interface::InterfaceConfiguration state_interface_configuration() const override { return controller_interface::InterfaceConfiguration{ From 5b5f33282fd887d9e2b91cd5aaf5443db7d7e6a7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 1 Feb 2024 12:37:47 +0100 Subject: [PATCH 5/6] remove unused joint_limits_interface package (#1353) --- joint_limits_interface/CMakeLists.txt | 47 -- joint_limits_interface/COLCON_IGNORE | 0 joint_limits_interface/README.md | 30 - .../joint_limits_interface.hpp | 718 ------------------ .../joint_limits_interface_exception.hpp | 36 - .../joint_limits_urdf.hpp | 88 --- joint_limits_interface/mainpage.dox | 142 ---- joint_limits_interface/package.xml | 36 - .../test/joint_limits_urdf_test.cpp | 176 ----- 9 files changed, 1273 deletions(-) delete mode 100644 joint_limits_interface/CMakeLists.txt delete mode 100644 joint_limits_interface/COLCON_IGNORE delete mode 100644 joint_limits_interface/README.md delete mode 100644 joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp delete mode 100644 joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp delete mode 100644 joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp delete mode 100644 joint_limits_interface/mainpage.dox delete mode 100644 joint_limits_interface/package.xml delete mode 100644 joint_limits_interface/test/joint_limits_urdf_test.cpp diff --git a/joint_limits_interface/CMakeLists.txt b/joint_limits_interface/CMakeLists.txt deleted file mode 100644 index a5d4577343..0000000000 --- a/joint_limits_interface/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ -cmake_minimum_required(VERSION 3.5.0) -project(joint_limits_interface) - -find_package(ament_cmake REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(rclcpp REQUIRED) -find_package(urdf REQUIRED) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) - - ament_add_gtest(joint_limits_interface_test test/joint_limits_interface_test.cpp) - target_include_directories(joint_limits_interface_test PUBLIC include) - ament_target_dependencies(joint_limits_interface_test hardware_interface rclcpp) - - add_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) - target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) - target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) - ament_target_dependencies(joint_limits_rosparam_test rclcpp) - add_launch_test(test/joint_limits_rosparam.launch.py) - install( - TARGETS - joint_limits_rosparam_test - DESTINATION lib/${PROJECT_NAME} - ) - install( - FILES - test/joint_limits_rosparam.yaml - DESTINATION share/${PROJECT_NAME}/test - ) - - ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp) - target_include_directories(joint_limits_urdf_test PUBLIC include) - ament_target_dependencies(joint_limits_urdf_test rclcpp urdf) -endif() - -# Install headers -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} -) - -ament_export_include_directories(include) -ament_export_dependencies(rclcpp urdf) - -ament_package() diff --git a/joint_limits_interface/COLCON_IGNORE b/joint_limits_interface/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/joint_limits_interface/README.md b/joint_limits_interface/README.md deleted file mode 100644 index c77ccc2b75..0000000000 --- a/joint_limits_interface/README.md +++ /dev/null @@ -1,30 +0,0 @@ -## Joint Limits Interface ## - -### Overview ### - -**joint_limits_interface** contains data structures for representing joint limits, methods for populating them from common -formats such as URDF and the ROS parameter server, and methods for enforcing limits on different kinds of hardware interfaces. - -The **joint_limits_interface** is *not* used by controllers themselves (it does not implement a `HardwareInterface`) but -instead operates after the controllers have updated, in the `write()` method (or equivalent) of the robot abstraction. -Enforcing limits will *overwrite* the commands set by the controllers, it does not operate on a separate raw data buffer. - -There are two main elements involved in setting up a joint limits interface: - - - **Joint limits representation** - - **JointLimits** Position, velocity, acceleration, jerk and effort. - - **SoftJointLimits** Soft position limits, `k_p`, `k_v` (as described [here](http://www.ros.org/wiki/pr2_controller_manager/safety_limits)). - - **Loading from URDF** There are convenience methods for loading joint limits information - (position, velocity and effort), as well as soft joint limits information from the URDF. - - **Loading from ROS params** There are convenience methods for loading joint limits from the ROS parameter server - (position, velocity, acceleration, jerk and effort). Parameter specification is the same used in MoveIt, - with the addition that we also parse jerk and effort limits. - - - **Joint limits interface** - - - For **effort-controlled** joints, the soft-limits implementation from the PR2 has been ported. - - For **position-controlled** joints, a modified version of the PR2 soft limits has been implemented. - - For **velocity-controlled** joints, simple saturation based on acceleration and velocity limits has been implemented. - -### Examples ### -Please refer to the [joint_limits_interface](https://github.com/ros-controls/ros_control/wiki/joint_limits_interface) wiki page. diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp deleted file mode 100644 index db553948d1..0000000000 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ /dev/null @@ -1,718 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ - -#include -#include -#include -#include -#include -#include - -#include "joint_limits_interface/joint_limits.hpp" -#include "joint_limits_interface/joint_limits_interface_exception.hpp" - -#include -#include - -#include -#include - -namespace joint_limits_interface -{ -/** - * The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint. - */ -class JointLimitHandle -{ -public: - JointLimitHandle() - : prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0), - jposh_(hardware_interface::HW_IF_POSITION), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_("position_command") - { - } - - JointLimitHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits) - : jposh_(jposh), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_(jcmdh), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - { - } - - JointLimitHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, const JointLimits & limits) - : jposh_(jposh), - jvelh_(jvelh), - jcmdh_(jcmdh), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - { - } - - /// \return Joint name. - std::string get_name() const - { - return jposh_ ? jposh_.get_name() - : jvelh_ ? jvelh_.get_name() - : jcmdh_ ? jcmdh_.get_name() - : std::string(); - } - - /// Sub-class implementation of limit enforcing policy. - virtual void enforce_limits(const rclcpp::Duration & period) = 0; - - /// Clear stored state, causing it to reset next iteration. - virtual void reset() - { - prev_pos_ = std::numeric_limits::quiet_NaN(); - prev_vel_ = 0.0; - } - -protected: - hardware_interface::JointHandle jposh_; - hardware_interface::JointHandle jvelh_; - hardware_interface::JointHandle jcmdh_; - joint_limits_interface::JointLimits limits_; - - // stored state - track position and velocity of last update - double prev_pos_; - double prev_vel_; - - /// Return velocity for limit calculations. - /** - * @param period Time since last measurement - * @return the velocity, from state if available, otherwise from previous position history. - */ - double get_velocity(const rclcpp::Duration & period) const - { - // if we have a handle to a velocity state we can directly return state velocity - // otherwise we will estimate velocity from previous position (command or state) - return jvelh_ ? jvelh_.get_value() : (jposh_.get_value() - prev_pos_) / period.seconds(); - } -}; - -/** The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint that has soft-limits. - */ -class JointSoftLimitsHandle : public JointLimitHandle -{ -public: - JointSoftLimitsHandle() {} - - JointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits, const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jcmdh, limits), soft_limits_(soft_limits) - { - } - - JointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, const JointLimits & limits, - const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jvelh, jcmdh, limits), soft_limits_(soft_limits) - { - } - -protected: - joint_limits_interface::SoftJointLimits soft_limits_; -}; - -/** A handle used to enforce position and velocity limits of a position-controlled joint that does - not have soft limits. */ -class PositionJointSaturationHandle : public JointLimitHandle -{ -public: - PositionJointSaturationHandle() {} - - PositionJointSaturationHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits) - : JointLimitHandle(jposh, jcmdh, limits) - { - if (limits_.has_position_limits) - { - min_pos_limit_ = limits_.min_position; - max_pos_limit_ = limits_.max_position; - } - else - { - min_pos_limit_ = -std::numeric_limits::max(); - max_pos_limit_ = std::numeric_limits::max(); - } - } - - /// Enforce position and velocity limits for a joint that is not subject to soft limits. - /** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - if (std::isnan(prev_pos_)) - { - prev_pos_ = jposh_.get_value(); - } - - double min_pos, max_pos; - if (limits_.has_velocity_limits) - { - // enforce velocity limits - // set constraints on where the position can be based on the - // max velocity times seconds since last update - const double delta_pos = limits_.max_velocity * period.seconds(); - min_pos = std::max(prev_pos_ - delta_pos, min_pos_limit_); - max_pos = std::min(prev_pos_ + delta_pos, max_pos_limit_); - } - else - { - // no velocity limit, so position is simply limited to set extents (our imposed soft limits) - min_pos = min_pos_limit_; - max_pos = max_pos_limit_; - } - - // clamp command position to our computed min/max position - const double cmd = std::clamp(jcmdh_.get_value(), min_pos, max_pos); - jcmdh_.set_value(cmd); - - prev_pos_ = cmd; - } - -private: - double min_pos_limit_, max_pos_limit_; -}; - -/// A handle used to enforce position and velocity limits of a position-controlled joint. -/** - * This class implements a very simple position and velocity limits enforcing policy, and tries to - * impose the least amount of requisites on the underlying hardware platform. This lowers - * considerably the entry barrier to use it, but also implies some limitations. - * - * Requisites - * - Position (for non-continuous joints) and velocity limits specification. - * - Soft limits specification. The \c k_velocity parameter is \e not used. - * - * Open loop nature - * - * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is - * checked for validity without relying on the actual position/velocity values. - * - * - Actual position values are \e not used because in some platforms there might be a substantial - * lag between sending a command and executing it (propagate command to hardware, reach control - * objective, read from hardware). - * - * - Actual velocity values are \e not used because of the above reason, and because some platforms - * might not expose trustworthy velocity measurements, or none at all. - * - * The downside of the open loop behavior is that velocity limits will not be enforced when - * recovering from large position tracking errors. Only the command is guaranteed to comply with the - * limits specification. - * - * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate - * the command velocity. - */ - -// TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? -class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - PositionJointSoftLimitsHandle() {} - - PositionJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jcmdh, limits, soft_limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce position and velocity limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity limits will be - * enforced. - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - assert(period.seconds() > 0.0); - - // Current position - if (std::isnan(prev_pos_)) - { - // Happens only once at initialization - prev_pos_ = jposh_.get_value(); - } - const double pos = prev_pos_; - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) - { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), -limits_.max_velocity, - limits_.max_velocity); - } - else - { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Position bounds - const double dt = period.seconds(); - double pos_low = pos + soft_min_vel * dt; - double pos_high = pos + soft_max_vel * dt; - - if (limits_.has_position_limits) - { - // This extra measure safeguards against pathological cases, like when the soft limit lies - // beyond the hard limit - pos_low = std::max(pos_low, limits_.min_position); - pos_high = std::min(pos_high, limits_.max_position); - } - - // Saturate position command according to bounds - const double pos_cmd = std::clamp(jcmdh_.get_value(), pos_low, pos_high); - jcmdh_.set_value(pos_cmd); - - // Cache variables - // todo: shouldn't this just be pos_cmd? why call into the command handle to - // get what we have in the above line? - prev_pos_ = jcmdh_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and effort limits of an effort-controlled - * joint that does not have soft limits. - */ -class EffortJointSaturationHandle : public JointLimitHandle -{ -public: - EffortJointSaturationHandle() {} - - EffortJointSaturationHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(jposh, jvelh, jcmdh, limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no efforts limits specification."); - } - } - - EffortJointSaturationHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : EffortJointSaturationHandle( - jposh, hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits) - { - } - - /** - * Enforce position, velocity, and effort limits for a joint that is not subject - * to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - double min_eff = -limits_.max_effort; - double max_eff = limits_.max_effort; - - if (limits_.has_position_limits) - { - const double pos = jposh_.get_value(); - if (pos < limits_.min_position) - { - min_eff = 0.0; - } - else if (pos > limits_.max_position) - { - max_eff = 0.0; - } - } - - const double vel = get_velocity(period); - if (vel < -limits_.max_velocity) - { - min_eff = 0.0; - } - else if (vel > limits_.max_velocity) - { - max_eff = 0.0; - } - - double clamped = std::clamp(jcmdh_.get_value(), min_eff, max_eff); - jcmdh_.set_value(clamped); - } -}; - -/// A handle used to enforce position, velocity and effort limits of an effort-controlled joint. -// TODO(anyone): This class is untested!. Update unit tests accordingly. -class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - EffortJointSoftLimitsHandle() {} - - EffortJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no effort limits specification."); - } - } - - EffortJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : EffortJointSoftLimitsHandle( - jposh, hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits, - soft_limits) - { - } - - /// Enforce position, velocity and effort limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity and effort limits - * will be enforced. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Current state - const double pos = jposh_.get_value(); - const double vel = get_velocity(period); - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) - { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), -limits_.max_velocity, - limits_.max_velocity); - } - else - { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Effort bounds depend on the velocity and effort bounds - const double soft_min_eff = std::clamp( - -soft_limits_.k_velocity * (vel - soft_min_vel), -limits_.max_effort, limits_.max_effort); - - const double soft_max_eff = std::clamp( - -soft_limits_.k_velocity * (vel - soft_max_vel), -limits_.max_effort, limits_.max_effort); - - // Saturate effort command according to bounds - const double eff_cmd = std::clamp(jcmdh_.get_value(), soft_min_eff, soft_max_eff); - jcmdh_.set_value(eff_cmd); - } -}; - -/// A handle used to enforce velocity and acceleration limits of a velocity-controlled joint. -class VelocityJointSaturationHandle : public JointLimitHandle -{ -public: - VelocityJointSaturationHandle() {} - - VelocityJointSaturationHandle( - const hardware_interface::JointHandle & jvelh, // currently unused - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle( - hardware_interface::JointHandle(hardware_interface::HW_IF_POSITION), jvelh, jcmdh, limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - VelocityJointSaturationHandle( - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle( - hardware_interface::JointHandle(hardware_interface::HW_IF_POSITION), - hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce joint velocity and acceleration limits. - /** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Velocity bounds - double vel_low; - double vel_high; - - if (limits_.has_acceleration_limits) - { - assert(period.seconds() > 0.0); - const double dt = period.seconds(); - - vel_low = std::max(prev_vel_ - limits_.max_acceleration * dt, -limits_.max_velocity); - vel_high = std::min(prev_vel_ + limits_.max_acceleration * dt, limits_.max_velocity); - } - else - { - vel_low = -limits_.max_velocity; - vel_high = limits_.max_velocity; - } - - // Saturate velocity command according to limits - const double vel_cmd = std::clamp(jcmdh_.get_value(), vel_low, vel_high); - jcmdh_.set_value(vel_cmd); - - // Cache variables - prev_vel_ = jcmdh_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and acceleration limits of a - * velocity-controlled joint. - */ -class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - VelocityJointSoftLimitsHandle() {} - - VelocityJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) - { - if (limits.has_velocity_limits) - { - max_vel_limit_ = limits.max_velocity; - } - else - { - max_vel_limit_ = std::numeric_limits::max(); - } - } - - /** - * Enforce position, velocity, and acceleration limits for a velocity-controlled joint - * subject to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - double min_vel, max_vel; - if (limits_.has_position_limits) - { - // Velocity bounds depend on the velocity limit and the proximity to the position limit. - const double pos = jposh_.get_value(); - min_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), -max_vel_limit_, - max_vel_limit_); - max_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), -max_vel_limit_, - max_vel_limit_); - } - else - { - min_vel = -max_vel_limit_; - max_vel = max_vel_limit_; - } - - if (limits_.has_acceleration_limits) - { - const double vel = get_velocity(period); - const double delta_t = period.seconds(); - min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel); - max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); - } - - jcmdh_.set_value(std::clamp(jcmdh_.get_value(), min_vel, max_vel)); - } - -private: - double max_vel_limit_; -}; - -// TODO(anyone): Port this to ROS 2 -// //** -// * Interface for enforcing joint limits. -// * -// * \tparam HandleType %Handle type. Must implement the following methods: -// * \code -// * void enforce_limits(); -// * std::string get_name() const; -// * \endcode -// */ -// template -// class joint_limits_interface::JointLimitsInterface -// : public hardware_interface::ResourceManager -// { -// public: -// HandleType getHandle(const std::string & name) -// { -// // Rethrow exception with a meaningful type -// try { -// return this->hardware_interface::ResourceManager::getHandle(name); -// } catch (const std::logic_error & e) { -// throw joint_limits_interface::JointLimitsInterfaceException(e.what()); -// } -// } -// -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Enforce limits for all managed handles. */ -// void enforce_limits(const rclcpp::Duration & period) -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.enforce_limits(period); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint through saturation. */ -// class PositionJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint with soft position limits. */ -// class PositionJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint through saturation. */ -// class EffortJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint with soft position limits. */ -// class EffortJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint through saturation. */ -// class VelocityJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint with soft position limits. */ -// class VelocityJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp deleted file mode 100644 index 430f46b48e..0000000000 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_ - -#include - -namespace joint_limits_interface -{ -/// An exception related to a \ref JointLimitsInterface -class JointLimitsInterfaceException : public std::exception -{ -public: - explicit JointLimitsInterfaceException(const std::string & message) : msg(message) {} - - const char * what() const noexcept override { return msg.c_str(); } - -private: - std::string msg; -}; - -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp deleted file mode 100644 index 80cf7f0ed4..0000000000 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_ - -#include -#include -#include -#include - -namespace joint_limits_interface -{ -/** - * Populate a JointLimits instance from URDF joint data. - * \param[in] urdf_joint URDF joint. - * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will - * overwrite existing values. Values in \e limits not present in \e urdf_joint remain unchanged. - * \return True if \e urdf_joint has a valid limits specification, false otherwise. - */ -inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) -{ - if (!urdf_joint || !urdf_joint->limits) - { - return false; - } - - limits.has_position_limits = - urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC; - if (limits.has_position_limits) - { - limits.min_position = urdf_joint->limits->lower; - limits.max_position = urdf_joint->limits->upper; - } - - if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS) - { - limits.angle_wraparound = true; - } - - limits.has_velocity_limits = true; - limits.max_velocity = urdf_joint->limits->velocity; - - limits.has_acceleration_limits = false; - - limits.has_effort_limits = true; - limits.max_effort = urdf_joint->limits->effort; - - return true; -} - -/** - * Populate a SoftJointLimits instance from URDF joint data. - * \param[in] urdf_joint URDF joint. - * \param[out] soft_limits Where URDF soft joint limit data gets written into. - * \return True if \e urdf_joint has a valid soft limits specification, false otherwise. - */ -inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits) -{ - if (!urdf_joint || !urdf_joint->safety) - { - return false; - } - - soft_limits.min_position = urdf_joint->safety->soft_lower_limit; - soft_limits.max_position = urdf_joint->safety->soft_upper_limit; - soft_limits.k_position = urdf_joint->safety->k_position; - soft_limits.k_velocity = urdf_joint->safety->k_velocity; - - return true; -} - -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_ diff --git a/joint_limits_interface/mainpage.dox b/joint_limits_interface/mainpage.dox deleted file mode 100644 index 9b23463bc3..0000000000 --- a/joint_limits_interface/mainpage.dox +++ /dev/null @@ -1,142 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -joint_limits_interface contains data structures for representing joint limits, methods for -populating them from common formats such as URDF and rosparam, and methods for enforcing limits on different kinds -of joint commands. - -The joint_limits_interface is \e not used by controllers themselves (it does not implement a \p HardwareInterface) but instead operates after the controllers have updated, in the \p write() method (or equivalent) of the robot abstraction. Enforcing limits will \e overwrite the commands set by the controllers, it does not operate on a separate raw data buffer. - -\section codeapi Code API - -There are two main elements involved in setting up a joint_limits_interface: - -\subsection limits_representation Joint limits representation - - - \ref joint_limits_interface::JointLimits "JointLimits" Position, velocity, acceleration, jerk and effort. - - \ref joint_limits_interface::SoftJointLimits "SoftJointLimits" Soft position limits, k_p, k_v (as described here ). - - \ref joint_limits_urdf.h "Convenience methods" for loading joint limits information (only position, velocity, effort), as well as soft joint limits information from the URDF. - - \ref joint_limits_rosparam.h "Convenience methods" for loading joint limits from ROS parameter server (all values). Parameter specification is the same used in MoveIt, with the addition that we also parse jerk and effort limits. - -\subsection limits_interface Joint limits interface - -For \b effort-controlled joints, \b position-controlled joints, and \b velocity-controlled joints, two types of interfaces have been created. The first is a saturation interface, used for joints that have normal limits but not soft limits. The second is an interface that implements soft limits, similar to the one used on the PR2. - - - Effort-controlled joints - - Saturation: \ref joint_limits_interface::EffortJointSaturationHandle "Handle", \ref joint_limits_interface::EffortJointSaturationInterface "Interface" - - Soft limits: \ref joint_limits_interface::EffortJointSoftLimitsHandle "Handle", \ref joint_limits_interface::EffortJointSoftLimitsInterface "Interface" - - Position-controlled joints - - Saturation: \ref joint_limits_interface::PositionJointSaturationHandle "Handle", \ref joint_limits_interface::PositionJointSaturationInterface "Interface" - - Soft limits: \ref joint_limits_interface::PositionJointSoftLimitsHandle "Handle", \ref joint_limits_interface::PositionJointSoftLimitsInterface "Interface" - - Velocity-controlled joints - - Saturation: \ref joint_limits_interface::VelocityJointSaturationHandle "Handle", \ref joint_limits_interface::VelocityJointSaturationInterface "interface" - - Soft limits: \ref joint_limits_interface::VelocityJointSoftLimitsHandle "Handle", \ref joint_limits_interface::VelocityJointSoftLimitsInterface "Interface" - -\section example Examples - -\subsection limits_representation_example Joint limits representation - -The first example shows the different ways of populating joint limits data structures. - -\code -#include - -#include -#include -#include - -int main(int argc, char** argv) -{ - // Init node handle and URDF model - ros::NodeHandle nh; - boost::shared_ptr urdf; - // ...initialize contents of urdf - - // Data structures - joint_limits_interface::JointLimits limits; - joint_limits_interface::SoftJointLimits soft_limits; - - // Manual value setting - limits.has_velocity_limits = true; - limits.max_velocity = 2.0; - - // Populate (soft) joint limits from URDF - // Limits specified in URDF overwrite existing values in 'limits' and 'soft_limits' - // Limits not specified in URDF preserve their existing values - urdf::JointConstSharedPtr urdf_joint = urdf->getJoint("foo_joint"); - const bool urdf_limits_ok = getJointLimits(urdf_joint, limits); - const bool urdf_soft_limits_ok = getSoftJointLimits(urdf_joint, soft_limits); - - // Populate (soft) joint limits from the ros parameter server - // Limits specified in the parameter server overwrite existing values in 'limits' and 'soft_limits' - // Limits not specified in the parameter server preserve their existing values - const bool rosparam_limits_ok = getJointLimits("foo_joint", nh, limits); -} -\endcode - -A joint limits specification in YAML format that can be loaded to the ROS parameter server can be found -\ref joint_limits_interface::getJointLimits(const std::string& joint_name, const ros::NodeHandle& nh, JointLimits& limits) "here". - -\subsection limits_interface_example Joint limits interface - -The second example integrates joint limits enforcing into an existing robot hardware implementation. - -\code -#include - -using namespace hardware_interface; -using joint_limits_interface::JointLimits; -using joint_limits_interface::SoftJointLimits; -using joint_limits_interface::PositionJointSoftLimitsHandle; -using joint_limits_interface::PositionJointSoftLimitsInterface; - -class MyRobot -{ -public: - MyRobot() {} - - bool init() - { - // Populate pos_cmd_interface_ with joint handles... - - // Get joint handle of interest - JointHandle joint_handle = pos_cmd_interface_.getHandle("foo_joint"); - - JointLimits limits; - SoftJointLimits soft_limits; - // Populate with any of the methods presented in the previous example... - - // Register handle in joint limits interface - PositionJointSoftLimitsHandle handle(joint_handle, // We read the state and read/write the command - limits, // Limits spec - soft_limits) // Soft limits spec - - jnt_limits_interface_.registerHandle(handle); - } - - void read(ros::Time time, ros::Duration period) - { - // Read actuator state from hardware... - - // Propagate current actuator state to joints... - } - - void write(ros::Time time, ros::Duration period) - { - // Enforce joint limits for all registered handles - // Note: one can also enforce limits on a per-handle basis: handle.enforce_limits(period) - jnt_limits_interface_.enforce_limits(period); - - // Propagate joint commands to actuators... - - // Send actuator command to hardware... - } - -private: - PositionJointInterface pos_cmd_interface_; - PositionJointSoftLimitsInterface jnt_limits_interface_; -}; -\endcode - -*/ diff --git a/joint_limits_interface/package.xml b/joint_limits_interface/package.xml deleted file mode 100644 index badbb51773..0000000000 --- a/joint_limits_interface/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - joint_limits_interface - 0.0.0 - Interface for enforcing joint limits. - - Bence Magyar - Denis Štogl - - Apache License 2.0 - - https://github.com/ros-controls/ros2_control/wiki - https://github.com/ros-controls/ros2_control/issues - https://github.com/ros-controls/ros2_control - - Adolfo Rodriguez Tsouroukdissian - - ament_cmake - - rclcpp - urdf - - hardware_interface - - hardware_interface - - ament_cmake_gtest - hardware_interface - launch - launch_ros - launch_testing - launch_testing_ament_cmake - - - ament_cmake - - diff --git a/joint_limits_interface/test/joint_limits_urdf_test.cpp b/joint_limits_interface/test/joint_limits_urdf_test.cpp deleted file mode 100644 index 55effc7117..0000000000 --- a/joint_limits_interface/test/joint_limits_urdf_test.cpp +++ /dev/null @@ -1,176 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/// \author Adolfo Rodriguez Tsouroukdissian - -#include - -#include - -#include - -class JointLimitsUrdfTest : public ::testing::Test -{ -public: - JointLimitsUrdfTest() - { - urdf_limits.reset(new urdf::JointLimits); - urdf_limits->effort = 8.0; - urdf_limits->velocity = 2.0; - urdf_limits->lower = -1.0; - urdf_limits->upper = 1.0; - - urdf_safety.reset(new urdf::JointSafety); - urdf_safety->k_position = 20.0; - urdf_safety->k_velocity = 40.0; - urdf_safety->soft_lower_limit = -0.8; - urdf_safety->soft_upper_limit = 0.8; - - urdf_joint.reset(new urdf::Joint); - urdf_joint->limits = urdf_limits; - urdf_joint->safety = urdf_safety; - - urdf_joint->type = urdf::Joint::UNKNOWN; - } - -protected: - urdf::JointLimitsSharedPtr urdf_limits; - urdf::JointSafetySharedPtr urdf_safety; - urdf::JointSharedPtr urdf_joint; -}; - -TEST_F(JointLimitsUrdfTest, GetJointLimits) -{ - // Unset URDF joint - { - joint_limits_interface::JointLimits limits; - urdf::JointSharedPtr urdf_joint_bad; - EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); - } - - // Unset URDF limits - { - joint_limits_interface::JointLimits limits; - urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); - EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); - } - - // Valid URDF joint, CONTINUOUS type - { - urdf_joint->type = urdf::Joint::CONTINUOUS; - - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits(urdf_joint, limits)); - - // Position - EXPECT_FALSE(limits.has_position_limits); - EXPECT_TRUE(limits.angle_wraparound); - - // Velocity - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); - - // Acceleration - EXPECT_FALSE(limits.has_acceleration_limits); - - // Effort - EXPECT_TRUE(limits.has_effort_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); - } - - // Valid URDF joint, REVOLUTE type - { - urdf_joint->type = urdf::Joint::REVOLUTE; - - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits(urdf_joint, limits)); - - // Position - EXPECT_TRUE(limits.has_position_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); - EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); - EXPECT_FALSE(limits.angle_wraparound); - - // Velocity - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); - - // Acceleration - EXPECT_FALSE(limits.has_acceleration_limits); - - // Effort - EXPECT_TRUE(limits.has_effort_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); - } - - // Valid URDF joint, PRISMATIC type - { - urdf_joint->type = urdf::Joint::PRISMATIC; - - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits(urdf_joint, limits)); - - // Position - EXPECT_TRUE(limits.has_position_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); - EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); - EXPECT_FALSE(limits.angle_wraparound); - - // Velocity - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); - - // Acceleration - EXPECT_FALSE(limits.has_acceleration_limits); - - // Effort - EXPECT_TRUE(limits.has_effort_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); - } -} - -TEST_F(JointLimitsUrdfTest, GetSoftJointLimits) -{ - // Unset URDF joint - { - joint_limits_interface::SoftJointLimits soft_limits; - urdf::JointSharedPtr urdf_joint_bad; - EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); - } - - // Unset URDF limits - { - joint_limits_interface::SoftJointLimits soft_limits; - urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); - EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); - } - - // Valid URDF joint - { - joint_limits_interface::SoftJointLimits soft_limits; - EXPECT_TRUE(getSoftJointLimits(urdf_joint, soft_limits)); - - // Soft limits - EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_lower_limit, soft_limits.min_position); - EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_upper_limit, soft_limits.max_position); - EXPECT_DOUBLE_EQ(urdf_joint->safety->k_position, soft_limits.k_position); - EXPECT_DOUBLE_EQ(urdf_joint->safety->k_velocity, soft_limits.k_velocity); - } -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From 26815e82f85468a1cfa4e206a58a86be1b3f692b Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Thu, 1 Feb 2024 19:47:22 +0100 Subject: [PATCH 6/6] [Doc] Add documentation about initial_value regarding mock_hw (#1352) --- .../doc/hardware_interface_types_userdoc.rst | 4 +++- .../doc/mock_components_userdoc.rst | 16 ++++++++++++++++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index 54b2003568..cd1e475b20 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -85,7 +85,9 @@ They can be combined together within the different hardware component types (sys - + + 3.1 + diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 4b54d5fa00..c075fdeafc 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -53,3 +53,19 @@ mimic (optional; string) multiplier (optional; double; default: 1; used if mimic joint is defined) Multiplier of values for mimicking joint defined in ``mimic`` parameter. Example: ``-2``. + +Per-interface Parameters +,,,,,,,,,,,,,,,,,,,,,,,, + +initial_value (optional; double) + Initial value of certain state interface directly after startup. Example: + + .. code-block:: xml + + + 3.45 + + + Note: This parameter is shared with the gazebo and gazebo classic plugins for + joint interfaces. For Mock components it is also possible to set initial + values for gpio or sensor state interfaces.