Transmission interface URDF parsing (imported from ddengster)

transmission_interface/CMakeLists.txt

@@ -18,17 +18,14 @@ find_package(TinyXML2 REQUIRED)
 
 add_library(transmission_parser SHARED src/transmission_parser.cpp)
 target_include_directories(transmission_parser PUBLIC include)
-ament_target_dependencies(transmission_parser
-  hardware_interface
-  TinyXML2
-)
+ament_target_dependencies(transmission_parser hardware_interface tinyxml2_vendor TinyXML2)
+ament_export_dependencies(hardware_interface tinyxml2_vendor TinyXML2)
 target_compile_definitions(transmission_parser PRIVATE "TRANSMISSION_INTERFACE_BUILDING_DLL")
 
 install(
   DIRECTORY include/
   DESTINATION include
 )
-
 install(
   TARGETS transmission_parser
   RUNTIME DESTINATION bin
@@ -45,6 +42,7 @@ if(BUILD_TESTING)
     test_transmission_parser
     test/test_transmission_parser.cpp
   )
+  target_include_directories(test_transmission_parser PUBLIC include)
   target_link_libraries(test_transmission_parser transmission_parser)
 endif()
 

transmission_interface/include/transmission_interface/transmission_info.hpp

@@ -15,18 +15,44 @@
 #ifndef TRANSMISSION_INTERFACE__TRANSMISSION_INFO_HPP_
 #define TRANSMISSION_INTERFACE__TRANSMISSION_INFO_HPP_
 
+#include <vector>
 #include <string>
 
-#include "hardware_interface/control_type.hpp"
 
 namespace transmission_interface
 {
 
+/**
+ * \brief Contains semantic info about a given joint loaded from XML (URDF)
+ */
+struct JointInfo
+{
+  std::string name;
+  std::vector<std::string> interfaces;
+  std::string role;
+};
+
+/**
+ * \brief Contains semantic info about a given actuator loaded from XML (URDF)
+ */
+struct ActuatorInfo
+{
+  std::string name;
+  std::vector<std::string> interfaces;
+  double mechanical_reduction;
+};
+
+/**
+ * \brief Contains semantic info about a given transmission loaded from XML (URDF)
+ */
 struct TransmissionInfo
 {
-  std::string joint_name;
-  std::string joint_control_type;
+  std::string name;
+  std::string type;
+  std::vector<JointInfo> joints;
+  std::vector<ActuatorInfo> actuators;
 };
 
 }  // namespace transmission_interface
+
 #endif  // TRANSMISSION_INTERFACE__TRANSMISSION_INFO_HPP_

transmission_interface/include/transmission_interface/transmission_parser.hpp

@@ -12,24 +12,56 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+/**
+ * \file
+ * \brief Parses <tt>\<transmission\></tt> elements into corresponding structs from XML (URDF).
+ * \author Dave Coleman
+ */
+
 #ifndef TRANSMISSION_INTERFACE__TRANSMISSION_PARSER_HPP_
 #define TRANSMISSION_INTERFACE__TRANSMISSION_PARSER_HPP_
 
+
+#include <transmission_interface/transmission_info.hpp>
+#include <transmission_interface/visibility_control.h>
+
+#include <tinyxml2.h>
+
 #include <string>
 #include <vector>
 
-#include "transmission_interface/transmission_info.hpp"
-#include "transmission_interface/visibility_control.h"
-
 namespace transmission_interface
 {
+
+/**
+ * \brief Parses the joint elements within transmission elements of a URDF
+ * If parse errors occur a std::runtime_error will be thrown with a description of the problem.
+ * \param[in] trans_it pointer to the current XML element being parsed
+ * \param[out] joints resulting list of joints in the transmission
+ * \return true if joint information for a transmission was successfully parsed.
+ */
+TRANSMISSION_INTERFACE_PUBLIC
+std::vector<JointInfo> parse_joints(tinyxml2::XMLElement * trans_it);
+
 /**
- * \brief Parse transmission information from a URDF
+ * \brief Parses the actuator elements within transmission elements of a URDF
+ * If parse errors occur a std::runtime_error will be thrown with a description of the problem.
+ * \param[in] trans_it pointer to the current XML element being parsed
+ * \param[out] actuators resulting list of actuators in the transmission
+ * \return true if actuator information for a transmission was successfully parsed.
+ */
+TRANSMISSION_INTERFACE_PUBLIC
+std::vector<ActuatorInfo> parse_actuators(tinyxml2::XMLElement * trans_it);
+
+
+/**
+ * \brief Parse transmission information from a URDF.
  * \param urdf A string containing the URDF xml
  * \return parsed transmission information
  * \throws std::runtime_error on malformed or empty xml
  */
 TRANSMISSION_INTERFACE_PUBLIC
 std::vector<TransmissionInfo> parse_transmissions_from_urdf(const std::string & urdf);
 }  // namespace transmission_interface
+
 #endif  // TRANSMISSION_INTERFACE__TRANSMISSION_PARSER_HPP_

transmission_interface/src/transmission_parser.cpp

@@ -12,19 +12,24 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "transmission_interface/transmission_parser.hpp"
-#include "transmission_interface/transmission_info.hpp"
+#include <transmission_interface/transmission_parser.hpp>
 
-#include <tinyxml2.h>
-#include <stdexcept>
+#include <sstream>
 #include <string>
 #include <vector>
 
 namespace
 {
+constexpr const auto kTransmissionParserLoggerName = "transmission_parser";
+
 constexpr const auto kTransmissionTag = "transmission";
+constexpr const auto kNameTag = "name";
 constexpr const auto kJointTag = "joint";
+constexpr const auto kActuatorTag = "actuator";
+constexpr const auto kTypeTag = "type";
+constexpr const auto kRoleTag = "role";
 constexpr const auto kHardwareInterfaceTag = "hardwareInterface";
+constexpr const auto kMechanicalReductionTag = "mechanicalReduction";
 }  // namespace
 
 namespace transmission_interface
@@ -42,38 +47,151 @@ std::vector<TransmissionInfo> parse_transmissions_from_urdf(const std::string &
   }
 
   std::vector<TransmissionInfo> transmissions;
-  // Find joints in transmission tags
-  const tinyxml2::XMLElement * root_it = doc.RootElement();
-  const tinyxml2::XMLElement * trans_it = root_it->FirstChildElement(kTransmissionTag);
-  while (trans_it) {
-    // Joint name
-    auto joint_it = trans_it->FirstChildElement(kJointTag);
-    if (!joint_it) {
-      throw std::runtime_error("no joint child element found");
+  tinyxml2::XMLElement * root_it = doc.RootElement();
+  tinyxml2::XMLElement * trans_it = nullptr;
+  for (trans_it = root_it->FirstChildElement(kTransmissionTag); trans_it;
+    trans_it = trans_it->NextSiblingElement(kTransmissionTag))
+  {
+    transmission_interface::TransmissionInfo transmission;
+
+    if (trans_it->Attribute(kNameTag)) {
+      transmission.name = trans_it->Attribute(kNameTag);
+      if (transmission.name.empty()) {
+        throw std::runtime_error("empty name attribute specified for transmission");
+      }
+    } else {
+      throw std::runtime_error("no name attribute specified for transmission");
+    }
+
+    // Transmission type
+    tinyxml2::XMLElement * type_child = trans_it->FirstChildElement(kTypeTag);
+    if (!type_child) {
+      throw std::runtime_error(
+              "no type element found in transmission '" + transmission.name + "'.");
     }
+    if (!type_child->GetText()) {
+      throw std::runtime_error(
+              "expected non-empty type element in transmission '" + transmission.name + "'.");
+    }
+    transmission.type = type_child->GetText();
+
+    try {
+      // Load joints
+      transmission.joints = parse_joints(trans_it);
+      // Load actuators
+      transmission.actuators = parse_actuators(trans_it);
+    } catch (const std::runtime_error & ex) {
+      // add the transmission name and rethrow
+      throw std::runtime_error("transmission '" + transmission.name + "' " + ex.what());
+    }
+
+    transmissions.push_back(transmission);
+  }
 
-    const std::string joint_name = joint_it->Attribute("name");
-    if (joint_name.empty()) {
+  return transmissions;
+}
+
+std::vector<JointInfo> parse_joints(tinyxml2::XMLElement * trans_it)
+{
+  std::vector<JointInfo> joints;
+  // Loop through each available joint
+  auto joint_it = trans_it->FirstChildElement(kJointTag);
+  if (!joint_it) {
+    throw std::runtime_error("no joint child element found");
+  }
+  for (; joint_it; joint_it = joint_it->NextSiblingElement(kJointTag)) {
+    transmission_interface::JointInfo joint;
+
+    joint.name = joint_it->Attribute(kNameTag);
+    if (joint.name.empty()) {
       throw std::runtime_error("no joint name attribute set");
     }
 
-    const auto hardware_interface_it = joint_it->FirstChildElement(kHardwareInterfaceTag);
-    if (!hardware_interface_it) {
+    const tinyxml2::XMLElement * role_it = joint_it->FirstChildElement(kRoleTag);
+    if (role_it) {
+      joint.role = role_it->GetText() ? role_it->GetText() : std::string();
+    }
+
+    // Interfaces (required)
+    auto interface_it = joint_it->FirstChildElement(kHardwareInterfaceTag);
+    if (!interface_it) {
+      throw std::runtime_error(
+              "no hardware interface tag found under transmission joint" + joint.name);
+    }
+
+    for (; interface_it; interface_it = interface_it->NextSiblingElement(kHardwareInterfaceTag)) {
+      const std::string interface_name = interface_it->GetText();
+      if (interface_name.empty()) {
+        throw std::runtime_error("no hardware interface specified in joint " + joint.name);
+      }
+      joint.interfaces.push_back(interface_name);
+    }
+
+    if (joint.interfaces.empty()) {
       throw std::runtime_error(
-              "no hardware interface tag found under transmission joint" + joint_name);
+              "joint " + joint.name + " has no valid hardware interface.");
     }
 
-    const std::string interface_name = hardware_interface_it->GetText();
-    if (interface_name.empty()) {
-      throw std::runtime_error("no hardware interface specified in joint " + joint_name);
+    joints.push_back(joint);
+  }
+
+  return joints;
+}
+
+std::vector<ActuatorInfo> parse_actuators(tinyxml2::XMLElement * trans_it)
+{
+  std::vector<ActuatorInfo> actuators;
+  // Loop through each available actuator
+  auto actuator_it = trans_it->FirstChildElement(kActuatorTag);
+  if (!actuator_it) {
+    throw std::runtime_error("no actuator child element found");
+  }
+
+  for (; actuator_it; actuator_it = actuator_it->NextSiblingElement(kActuatorTag)) {
+    transmission_interface::ActuatorInfo actuator;
+
+    actuator.name = actuator_it->Attribute(kNameTag);
+    if (actuator.name.empty()) {
+      throw std::runtime_error("no actuator name attribute set");
     }
 
-    transmissions.push_back({joint_name, interface_name});
+    // Hardware interfaces (optional)
+    auto interface_it = actuator_it->FirstChildElement(kHardwareInterfaceTag);
+    if (!interface_it) {
+      throw std::runtime_error(
+              "no hardware interface tag found under transmission actuator" + actuator.name);
+    }
+
+    for (; interface_it; interface_it = interface_it->NextSiblingElement(kHardwareInterfaceTag)) {
+      const std::string interface_name = interface_it->GetText();
+      if (interface_name.empty()) {
+        throw std::runtime_error("no hardware interface specified in actuator " + actuator.name);
+      }
+      actuator.interfaces.push_back(interface_name);
+    }
+
+    if (actuator.interfaces.empty()) {
+      throw std::runtime_error(
+              "actuator " + actuator.name + " has no valid hardware interface.");
+    }
 
-    trans_it = trans_it->NextSiblingElement(kTransmissionTag);
+    // mechanical reduction (optional)
+    actuator.mechanical_reduction = 1;
+    auto mechred_it = actuator_it->FirstChildElement(kMechanicalReductionTag);
+    for (; mechred_it; mechred_it = mechred_it->NextSiblingElement(kMechanicalReductionTag)) {
+      // optional tag but if specified, it should not be empty!
+      const std::string mech_red_str = mechred_it->GetText();
+      if (mech_red_str.empty()) {
+        throw std::runtime_error("mechanical reduction tag was specified without value");
+      } else {
+        actuator.mechanical_reduction = atoi(mech_red_str.c_str());
+      }
+    }
+
+    actuators.push_back(actuator);
   }
 
-  return transmissions;
+  return actuators;
 }
 
 }  // namespace transmission_interface