[HWItfs] Add key-value-storage to the InterfaceInfo (#1421)

* add key-value storage to the IntefaceInfo, parameters can now be defined per Command-/StateInterface

---------

Co-authored-by: Daniel Azanov <31107191+muritane@users.noreply.github.com>
Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>

doc/release_notes.rst

@@ -104,6 +104,7 @@ hardware_interface
 * Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 <https://github.com/ros-controls/ros2_control/pull/1585>`_)
 * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 <https://github.com/ros-controls/ros2_control/pull/1643>`_)
 * With (`#1683 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/1240>`_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles.
+* With (`#1421 <https://github.com/ros-controls/ros2_control/pull/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
 ************

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 <https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/hardware_info.hpp>`_.
+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
+
+    <ros2_control name="Name_of_the_hardware" type="system">
+      <hardware>
+        <plugin>library_name/ClassName</plugin>
+        <!-- added to hardware_parameters -->
+        <param name="example_param">value</param>
+      </hardware>
+      <joint name="name_of_the_component">
+        <command_interface name="interface_name">
+          <!-- All of them are optional. `data_type` and `size` are used for GPIOs. Size is length of an array. -->
+          <param name="min">-1</param>
+          <param name="max">1</param>
+          <param name="initial_value">0.0</param>
+          <param name="data_type"></param>
+          <param name="size">5</param>
+           <!-- Optional. Added to the key/value storage parameters -->
+          <param name="own_param_1">some_value</param>
+          <param name="own_param_2">other_value</param>
+        </command_interface>
+        <!-- Short form to define StateInterface. Can be extended like CommandInterface. -->
+        <state_interface name="position"/>
+      </joint>
+    </ros2_control>
+
 Joints
 *****************************
 ``<joint>``-tag groups the interfaces associated with the joints of physical robots and actuators.

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, etc.
+  std::unordered_map<std::string, std::string> parameters;
 };
 
 /// @brief This structure stores information about a joint that is mimicking another joint

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;
 }
 

hardware_interface/test/test_component_parser.cpp

@@ -297,6 +297,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 =

ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp

@@ -43,7 +43,7 @@ const auto valid_urdf_ros2_control_system_one_interface =
   </ros2_control>
 )";
 
-// 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"(
@@ -84,6 +84,70 @@ const auto valid_urdf_ros2_control_system_multi_interface =
   </ros2_control>
 )";
 
+// 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 name="RRBotSystemMultiInterface" type="system">
+    <hardware>
+      <plugin>ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware</plugin>
+      <param name="example_param_write_for_sec">2</param>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <joint name="joint1">
+      <command_interface name="position">
+        <param name="min">-1</param>
+        <param name="max">1</param>
+        <param name="initial_value">1.2</param>
+        <param name="register">1</param>
+        <param name="register_size">2</param>
+      </command_interface>
+      <command_interface name="velocity">
+        <param name="min">-1.5</param>
+        <param name="max">1.5</param>
+        <param name="initial_value">3.4</param>
+        <param name="register">2</param>
+        <param name="register_size">4</param>
+      </command_interface>
+      <command_interface name="effort">
+        <param name="min">-0.5</param>
+        <param name="max">0.5</param>
+      </command_interface>
+      <state_interface name="position">
+        <param name="register">3</param>
+        <param name="register_size">2</param>
+      </state_interface>
+      <state_interface name="velocity">
+        <param name="register">4</param>
+        <param name="register_size">4</param>
+      </state_interface>
+      <state_interface name="effort"/>
+      <param name="modbus_server_ip">1.1.1.1</param>
+      <param name="modbus_server_port">1234</param>
+      <param name="use_persistent_connection">true</param>
+    </joint>
+    <joint name="joint2">
+      <command_interface name="position">
+        <param name="min">-1</param>
+        <param name="max">1</param>
+        <param name="register">20</param>
+        <param name="data_type">int32_t</param>
+      </command_interface>
+      <state_interface name="position">
+        <param name="register">21</param>
+        <param name="data_type">int32_t</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort">
+        <param name="register">21</param>
+        <param name="data_type">int32_t</param>
+      </state_interface>
+      <param name="modbus_server_ip">192.168.178.123</param>
+      <param name="modbus_server_port">4321</param>
+    </joint>
+  </ros2_control>
+)";
+
 // 3. Industrial Robots with integrated sensor
 const auto valid_urdf_ros2_control_system_robot_with_sensor =
   R"(