From d8b9eb76cbdd3bacc3b583d15f997f58206482a2 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 27 Feb 2024 18:43:52 +0100 Subject: [PATCH 1/9] add key-value storage to the IntefaceInfo, parameters can now be defined per Command-/StateInterface --- .../hardware_interface/hardware_info.hpp | 4 + hardware_interface/src/component_parser.cpp | 15 ++- .../test/test_component_parser.cpp | 102 ++++++++++++++++++ .../components_urdfs.hpp | 66 +++++++++++- 4 files changed, 184 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index d94573bf5e..acfa0a63ca 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -46,6 +46,10 @@ struct InterfaceInfo int size; /// (Optional) enable or disable the limits for the command interfaces bool enable_limits; + /// (Optional) Key-value pairs of command/stateInterface parameters. This is + /// useful for drivers that operate on protocols like modbus, where each + /// interface needs own address(register), datatype, ... + std::unordered_map parameters; }; /// @brief This structure stores information about a joint that is mimicking another joint diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 14e016df21..381da7a28f 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -313,6 +313,13 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( interface.data_type = "double"; interface.size = 1; + // Parse parameters + const auto * params_it = interfaces_it->FirstChildElement(kParamTag); + if (params_it) + { + interface.parameters = parse_parameters_from_xml(params_it); + } + return interface; } @@ -791,11 +798,15 @@ std::vector parse_control_resources_from_urdf(const std::string & tinyxml2::XMLDocument doc; if (!doc.Parse(urdf.c_str()) && doc.Error()) { - throw std::runtime_error("invalid URDF passed in to robot parser"); + throw std::runtime_error( + "invalid URDF passed in to robot parser. Could not parse it because of following error:" + + std::string(doc.ErrorStr())); } if (doc.Error()) { - throw std::runtime_error("invalid URDF passed in to robot parser"); + throw std::runtime_error( + "invalid URDF passed in to robot parser. Following error occurred:" + + std::string(doc.ErrorStr())); } // Find robot tag diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 6a0c11cf72..d268ea61d1 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -294,6 +294,108 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens } } +TEST_F( + TestComponentParser, + successfully_parse_valid_urdf_system_multi_interface_custom_interface_parameters) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets:: + valid_urdf_ros2_control_system_multi_interface_and_custom_interface_parameters + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + const auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "RRBotSystemMultiInterface"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, + "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware"); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + EXPECT_EQ(hardware_info.joints[0].parameters.size(), 3); + EXPECT_EQ(hardware_info.joints[0].parameters.at("modbus_server_ip"), "1.1.1.1"); + EXPECT_EQ(hardware_info.joints[0].parameters.at("modbus_server_port"), "1234"); + EXPECT_EQ(hardware_info.joints[0].parameters.at("use_persistent_connection"), "true"); + ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(3)); + ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(3)); + // CommandInterfaces of joints + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].initial_value, "1.2"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.size(), 5); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.at("register"), "1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.at("register_size"), "2"); + + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].initial_value, "3.4"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].min, "-1.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].max, "1.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.size(), 5); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.at("register"), "2"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.at("register_size"), "4"); + + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].min, "-0.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].max, "0.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].initial_value, ""); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].parameters.size(), 2); + + // StateInterfaces of joints + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.at("register"), "3"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.at("register_size"), "2"); + + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.at("register"), "4"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.at("register_size"), "4"); + + EXPECT_EQ(hardware_info.joints[0].state_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[2].parameters.size(), 0); + + // Second Joint + EXPECT_EQ(hardware_info.joints[1].name, "joint2"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_EQ(hardware_info.joints[1].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].parameters.at("modbus_server_ip"), "192.168.178.123"); + EXPECT_EQ(hardware_info.joints[1].parameters.at("modbus_server_port"), "4321"); + ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); + ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3)); + // CommandInterfaces + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].initial_value, ""); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-1"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.size(), 4); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.at("register"), "20"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.at("data_type"), "int32_t"); + // StateInterfaces of joints + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.at("register"), "21"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.at("data_type"), "int32_t"); + + EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].parameters.size(), 0); + + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.at("register"), "21"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.at("data_type"), "int32_t"); +} + TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor) { std::string urdf_to_test = 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 7b46eda9c0..90d5785e33 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 @@ -45,7 +45,7 @@ const auto valid_urdf_ros2_control_system_one_interface = )"; -// 2. Industrial Robots with multiple interfaces (cannot be written at the same time) +// 2.1 Industrial Robots with multiple interfaces (cannot be written at the same time) // Note, joint parameters can be any string const auto valid_urdf_ros2_control_system_multi_interface = R"( @@ -86,6 +86,70 @@ const auto valid_urdf_ros2_control_system_multi_interface = )"; +// 2.2 Industrial Robots with multiple interfaces (cannot be written at the same time) +// Additionally some of the Command-/StateInterfaces have parameters defined per interface +const auto valid_urdf_ros2_control_system_multi_interface_and_custom_interface_parameters = + R"( + + + ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware + 2 + 2 + + + + -1 + 1 + 1.2 + 1 + 2 + + + -1.5 + 1.5 + 3.4 + 2 + 4 + + + -0.5 + 0.5 + + + 3 + 2 + + + 4 + 4 + + + 1.1.1.1 + 1234 + true + + + + -1 + 1 + 20 + int32_t + + + 21 + int32_t + + + + 21 + int32_t + + 192.168.178.123 + 4321 + + +)"; + // 3. Industrial Robots with integrated sensor const auto valid_urdf_ros2_control_system_robot_with_sensor = R"( From 17216a88dfe71bf3747715210ba3ba80881b5aab Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 22 May 2024 11:02:56 +0200 Subject: [PATCH 2/9] add overview and usage of key-value storage to docs --- .../doc/hardware_interface_types_userdoc.rst | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index ecf852cb94..f454caa969 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -9,6 +9,38 @@ The ``ros2_control`` framework provides a set of hardware interface types that c a hardware component for a specific robot or device. The following sections describe the different hardware interface types and their usage. +Overview +***************************** +Hardware in ros2_control is described as URDF and internally parsed and encapsulated as ``HardwareInfo``. +The definition can be found in the `ros2_control repository `_. +You can check this classes to see what attributes are available for each of the xml tags. +A generic example which shows the structure is provided below. More specific examples can be found in the Example part below. + +.. code:: xml + + + + library_name/ClassName + + value + + + + + -1 + 1 + 0.0 + + 5 + + some_value + other_value + + + + + + Joints ***************************** ````-tag groups the interfaces associated with the joints of physical robots and actuators. From 7d7c3f5ddeb58a2f823fd8f7f567479a01a9ffe0 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Fri, 16 Aug 2024 14:19:56 +0200 Subject: [PATCH 3/9] Update hardware_interface/doc/hardware_interface_types_userdoc.rst Co-authored-by: Dr. Denis --- hardware_interface/doc/hardware_interface_types_userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index 9a89604f24..4b881d4125 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -26,7 +26,7 @@ A generic example which shows the structure is provided below. More specific exa - + -1 1 0.0 From 81836ecbd9660754028f8ad91115bdf58f0b116b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Fri, 16 Aug 2024 14:21:06 +0200 Subject: [PATCH 4/9] Fix formatting. --- hardware_interface/doc/hardware_interface_types_userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index 4b881d4125..b29a8d09a0 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -11,7 +11,7 @@ The following sections describe the different hardware interface types and their Overview ***************************** -Hardware in ros2_control is described as URDF and internally parsed and encapsulated as ``HardwareInfo``. +Hardware in ros2_control is described as URDF and internally parsed and encapsulated as ``HardwareInfo``. The definition can be found in the `ros2_control repository `_. You can check this classes to see what attributes are available for each of the xml tags. A generic example which shows the structure is provided below. More specific examples can be found in the Example part below. From da49ea2c937d9d142c73dc9c52a9a4fa06c750f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Fri, 16 Aug 2024 14:29:51 +0200 Subject: [PATCH 5/9] Fix formatting. --- hardware_interface/doc/hardware_interface_types_userdoc.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index b29a8d09a0..d2a2f078d9 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -18,13 +18,13 @@ A generic example which shows the structure is provided below. More specific exa .. code:: xml - + library_name/ClassName value - + -1 @@ -38,7 +38,7 @@ A generic example which shows the structure is provided below. More specific exa - + Joints From fe055b9a764738374791ef6ef3f63e179b8a92df Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Mon, 19 Aug 2024 09:22:47 +0200 Subject: [PATCH 6/9] Update hardware_interface/doc/hardware_interface_types_userdoc.rst --- hardware_interface/doc/hardware_interface_types_userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index d2a2f078d9..45141e5479 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -13,7 +13,7 @@ Overview ***************************** Hardware in ros2_control is described as URDF and internally parsed and encapsulated as ``HardwareInfo``. The definition can be found in the `ros2_control repository `_. -You can check this classes to see what attributes are available for each of the xml tags. +You can check the structs defined there to see what attributes are available for each of the xml tags. A generic example which shows the structure is provided below. More specific examples can be found in the Example part below. .. code:: xml From a8aa5ad198ed8e72a86e272ef1dfe895892690c3 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Tue, 27 Aug 2024 11:08:57 +0200 Subject: [PATCH 7/9] Update hardware_interface/include/hardware_interface/hardware_info.hpp Co-authored-by: Bence Magyar --- hardware_interface/include/hardware_interface/hardware_info.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 49be2f4d02..eea8b6ca8a 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -48,7 +48,7 @@ struct InterfaceInfo bool enable_limits; /// (Optional) Key-value pairs of command/stateInterface parameters. This is /// useful for drivers that operate on protocols like modbus, where each - /// interface needs own address(register), datatype, ... + /// interface needs own address(register), datatype, etc. std::unordered_map parameters; }; From c78bac7570e4611b09f6ad9e1aeb721101cb32b3 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 28 Aug 2024 11:42:52 +0200 Subject: [PATCH 8/9] update release notes --- doc/release_notes.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 50583b3bf4..6cf604438b 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -105,6 +105,10 @@ hardware_interface * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. +InterfaceInfo +************* +* With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. + joint_limits ************ * Add header to import limits from standard URDF definition (`#1298 `_) From b69068fb53586d0bfb4d55d059eb07dbe21df9ea Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 28 Aug 2024 13:50:59 +0200 Subject: [PATCH 9/9] Update doc/release_notes.rst Co-authored-by: Sai Kishor Kothakota --- doc/release_notes.rst | 3 --- 1 file changed, 3 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 6cf604438b..097ab208b0 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -104,9 +104,6 @@ hardware_interface * Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. - -InterfaceInfo -************* * With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. joint_limits