From c548f8b3d9406b2251ce980a6f2c83814e813516 Mon Sep 17 00:00:00 2001
From: "Felix Exner (fexner)" <git@fexner.de>
Date: Tue, 30 May 2023 15:08:22 +0200
Subject: [PATCH] Empty urdf tag humble (backport of #1017) (#1036)

* [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.
* Patch test updates for humble API

---------

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

diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp
index c5bd6cadcf..c05ec8ca47 100644
--- a/hardware_interface/src/component_parser.cpp
+++ b/hardware_interface/src/component_parser.cpp
@@ -13,6 +13,8 @@
 // limitations under the License.
 
 #include <tinyxml2.h>
+#include <charconv>
+#include <iostream>
 #include <regex>
 #include <stdexcept>
 #include <string>
@@ -65,7 +67,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 363d5126b7..b19c10169e 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 =
@@ -598,6 +588,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_class_type,
+    "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 73dc32dd2f..816c229e43 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">