add key-value storage to the IntefaceInfo, parameters can now be defi…

…ned per Command-/StateInterface
ros-controls · May 22, 2024 · d8b9eb7 · d8b9eb7
1 parent 62b32af
commit d8b9eb7
Show file tree

Hide file tree

Showing 4 changed files with 184 additions and 3 deletions.
diff --git 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<std::string, std::string> 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
@@ -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<HardwareInfo> 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
@@ -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
@@ -45,7 +45,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"(
@@ -86,6 +86,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"(