From 7c074302269254e3ade2639c698473351877a595 Mon Sep 17 00:00:00 2001
From: "Felix Exner (fexner)" <exner@fzi.de>
Date: Fri, 19 May 2023 22:08:25 +0200
Subject: [PATCH] [URDF Parser] Allow empty urdf tag, e.g., parameter (#1017)

* Do not explode with empty tag
* Update tests to allow empty URDF parameter to hardware interface
* Test checks that empty parameter tags work and actually checks the parsed urdf.

---------

Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
---
 hardware_interface/src/component_parser.cpp   |  4 ++-
 .../test/test_component_parser.cpp            | 36 +++++++++++++------
 .../components_urdfs.hpp                      | 32 ++++++++---------
 3 files changed, 44 insertions(+), 28 deletions(-)

diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp
index 46c6b5f0e0..305c7ff7aa 100644
--- a/hardware_interface/src/component_parser.cpp
+++ b/hardware_interface/src/component_parser.cpp
@@ -14,6 +14,7 @@
 
 #include <tinyxml2.h>
 #include <charconv>
+#include <iostream>
 #include <regex>
 #include <stdexcept>
 #include <string>
@@ -68,7 +69,8 @@ std::string get_text_for_element(
   const auto get_text_output = element_it->GetText();
   if (!get_text_output)
   {
-    throw std::runtime_error("text not specified in the " + tag_name + " tag");
+    std::cerr << "text not specified in the " << tag_name << " tag" << std::endl;
+    return "";
   }
   return get_text_output;
 }
diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp
index 97d4a77d21..b0c7c5a16d 100644
--- a/hardware_interface/test/test_component_parser.cpp
+++ b/hardware_interface/test/test_component_parser.cpp
@@ -100,16 +100,6 @@ TEST_F(TestComponentParser, component_interface_type_empty_throws_error)
   ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error);
 }
 
-TEST_F(TestComponentParser, parameter_empty_throws_error)
-{
-  const std::string broken_urdf_string =
-    std::string(ros2_control_test_assets::urdf_head) +
-    ros2_control_test_assets::invalid_urdf_ros2_control_parameter_empty +
-    ros2_control_test_assets::urdf_tail;
-
-  ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error);
-}
-
 TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface)
 {
   std::string urdf_to_test =
@@ -625,6 +615,32 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d
   EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1);
 }
 
+TEST_F(TestComponentParser, successfully_parse_parameter_empty)
+{
+  const std::string urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head) +
+    ros2_control_test_assets::valid_urdf_ros2_control_parameter_empty +
+    ros2_control_test_assets::urdf_tail;
+  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
+  ASSERT_THAT(control_hardware, SizeIs(1));
+  auto hardware_info = control_hardware.front();
+
+  EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_Position_Only");
+  EXPECT_EQ(hardware_info.type, "system");
+  EXPECT_EQ(
+    hardware_info.hardware_plugin_name,
+    "ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only");
+
+  ASSERT_THAT(hardware_info.joints, SizeIs(1));
+
+  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
+  EXPECT_EQ(hardware_info.joints[0].type, "joint");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "position");
+
+  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "");
+  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2");
+}
+
 TEST_F(TestComponentParser, negative_size_throws_error)
 {
   std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) +
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 f3e2bda2c0..a42d39a241 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
@@ -398,6 +398,21 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type =
   </ros2_control>
 )";
 
+const auto valid_urdf_ros2_control_parameter_empty =
+  R"(
+  <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
+    <hardware>
+      <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
+      <param name="example_param_write_for_sec"></param>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <joint name="joint1">
+      <command_interface name="position">
+      </command_interface>
+    </joint>
+  </ros2_control>
+)";
+
 // Errors
 const auto invalid_urdf_ros2_control_invalid_child =
   R"(
@@ -485,23 +500,6 @@ const auto invalid_urdf_ros2_control_component_interface_type_empty =
   </ros2_control>
 )";
 
-const auto invalid_urdf_ros2_control_parameter_empty =
-  R"(
-  <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
-    <hardware>
-      <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
-      <param name="example_param_write_for_sec"></param>
-      <param name="example_param_read_for_sec">2</param>
-    </hardware>
-    <joint name="joint1">
-      <command_interface name="position">
-        <param name="min_position_value">-1</param>
-        <param name="max_position_value">1</param>
-      </command_interface>
-    </joint>
-  </ros2_control>
-)";
-
 const auto invalid_urdf2_ros2_control_illegal_size =
   R"(
   <ros2_control name="RRBotSystemWithIllegalSize" type="system">