Allow multiple hw interfaces, Fix ros-controls#112, and test.

- Allow to specify multiple hardware interfaces for joints and actuators.

- Fix invalid xml_element tag. Contents are now stored as a string.

- Unit test parser.

transmission_interface/CMakeLists.txt

@@ -12,12 +12,27 @@ if(USE_ROSBUILD)
 
   # Transmission Paraser Library
   rosbuild_add_library(${PROJECT_NAME}_parser src/transmission_parser.cpp)
-  target_link_libraries(${PROJECT_NAME}_parser)
+
+  rosbuild_add_library(transmission_interface_loader
+    src/transmission_loader.cpp           include/transmission_interface/transmission_loader.h
+    src/simple_transmission_loader.cpp    include/transmission_interface/simple_transmission_loader.h
+    src/transmission_interface_loader.cpp include/transmission_interface/transmission_interface_loader.h)
+
+  rosbuild_add_library(simple_transmission_loader src/simple_transmission_loader.cpp)
+  target_link_libraries(simple_transmission_loader           transmission_interface_loader)
 
   rosbuild_add_gtest(simple_transmission_test           test/simple_transmission_test.cpp)
   rosbuild_add_gtest(differential_transmission_test     test/differential_transmission_test.cpp)
   rosbuild_add_gtest(four_bar_linkage_transmission_test test/four_bar_linkage_transmission_test.cpp)
   rosbuild_add_gtest(transmission_interface_test        test/transmission_interface_test.cpp)
+  rosbuild_add_gtest(transmission_parser_test           test/transmission_parser_test.cpp)
+  rosbuild_add_gtest(simple_transmission_loader_test    test/simple_transmission_loader_test.cpp)
+  rosbuild_add_gtest(transmission_interface_loader_test test/transmission_interface_loader_test.cpp)
+
+  target_link_libraries(transmission_parser_test           ${PROJECT_NAME}_parser)
+  target_link_libraries(simple_transmission_loader_test    ${PROJECT_NAME}_parser)
+  target_link_libraries(transmission_interface_loader_test ${PROJECT_NAME}_parser
+                                                           transmission_interface_loader)
 
   # TODO: why is it explicitly needed???, without it linker fails.
   target_link_libraries(simple_transmission_test           pthread)
@@ -31,6 +46,7 @@ else()
   find_package(catkin REQUIRED
     hardware_interface
     cmake_modules
+    resource_retriever
   )
 
   # Include a custom cmake file for TinyXML
@@ -44,6 +60,7 @@ else()
       include
     DEPENDS
       TinyXML
+      resource_retriever
   )
 
 ###########
@@ -93,5 +110,8 @@ else()
     target_link_libraries(transmission_interface_test ${catkin_LIBRARIES} ${TinyXML_LIBRARIES})
   endif()
 
+  catkin_add_gtest(transmission_parser_test           test/transmission_parser_test.cpp)
+  target_link_libraries(transmission_parser_test   ${PROJECT_NAME}_parser)
+
 
 endif()

transmission_interface/include/transmission_interface/transmission_info.h

@@ -41,10 +41,11 @@
 #ifndef TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_INFO_H
 #define TRANSMISSION_INTERFACE_TRANSMISSION_INTERFACE_INFO_H
 
+// C++ standard
 #include <vector>
 #include <string>
 
-// XML
+// TinyXML
 #include <tinyxml.h>
 
 namespace transmission_interface
@@ -56,18 +57,19 @@ namespace transmission_interface
 struct JointInfo
 {
   std::string name_;
-  std::string hardware_interface_;
+  std::vector<std::string> hardware_interfaces_;
   std::string role_;
-  TiXmlElement* xml_element_;
+  std::string xml_element_;
 };
 
 /**
  * \brief Contains semantic info about a given actuator loaded from XML (URDF)
  */
-struct ActuatorInfo {
+struct ActuatorInfo
+{
   std::string name_;
-  std::string hardware_interface_;
-  TiXmlElement* xml_element_;
+  std::vector<std::string> hardware_interfaces_;
+  std::string xml_element_;
 };
 
 /**

transmission_interface/manifest.xml.ignore

@@ -10,10 +10,15 @@
   <rosdep name="tinyxml" />
 
   <depend package="hardware_interface" />
+  <depend package="pluginlib" />
+  <depend package="roscpp" />
+
+<depend package="resource_retriever" /> <!--Tests only-->
 
   <export>
     <cpp cflags="-I${prefix}/include" />
     <rosdoc config="rosdoc.yaml" />
+    <transmission_interface plugin="${prefix}/ros_control_plugins.xml" />
   </export>
 
 </package>

transmission_interface/package.xml

@@ -19,11 +19,16 @@
   <build_depend>hardware_interface</build_depend>
   <build_depend>tinyxml</build_depend>
   <build_depend>cmake_modules</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>resource_retriever</build_depend> <!-- Test-only-->
 
   <run_depend>tinyxml</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>resource_retriever</run_depend> <!-- Test-only-->
 
   <export>
     <cpp cflags="-I${prefix}/include"/>
     <rosdoc config="rosdoc.yaml"/>
+    <transmission_interface plugin="${prefix}/ros_control_plugins.xml" />
   </export>
 </package>

transmission_interface/src/transmission_parser.cpp

@@ -33,6 +33,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
+#include <sstream>
 #include <transmission_interface/transmission_parser.h>
 
 namespace transmission_interface
@@ -44,8 +45,7 @@ bool TransmissionParser::parse(const std::string& urdf, std::vector<Transmission
   TiXmlDocument doc;
   if (!doc.Parse(urdf.c_str()) && doc.Error())
   {
-    ROS_ERROR("Could not load the gazebo_ros_control plugin's"
-      " configuration file: %s\n", urdf.c_str());
+    ROS_ERROR("Can't parse transmissions. Invalid URDF.");
     return false;
   }
 
@@ -63,42 +63,47 @@ bool TransmissionParser::parse(const std::string& urdf, std::vector<Transmission
     if(trans_it->Attribute("name"))
     {
       transmission.name_ = trans_it->Attribute("name");
+      if (transmission.name_.empty())
+      {
+        ROS_ERROR_STREAM_NAMED("parser","Empty name attribute specified for transmission.");
+        continue;
+      }
     }
     else
     {
-      ROS_ERROR_STREAM_NAMED("parser","No name attribute for transmission tag!");
+      ROS_ERROR_STREAM_NAMED("parser","No name attribute specified for transmission.");
       continue;
     }
 
     // Transmission type
     TiXmlElement *type_child = trans_it->FirstChildElement("type");
     if(!type_child)
     {
-      ROS_ERROR_STREAM_NAMED("parser","No type element found for transmission "
-        << transmission.name_);
+      ROS_ERROR_STREAM_NAMED("parser","No type element found in transmission '"
+        << transmission.name_ << "'.");
       continue;
     }
-    transmission.type_ = type_child->GetText();
-    if(transmission.type_.empty())
+    if (!type_child->GetText())
     {
-      ROS_ERROR_STREAM_NAMED("parser","No type string found for transmission "
-        << transmission.name_);
+      ROS_ERROR_STREAM_NAMED("parser","Skipping empty type element in transmission '"
+                             << transmission.name_ << "'.");
       continue;
     }
+    transmission.type_ = type_child->GetText();
 
     // Load joints
     if(!parseJoints(trans_it, transmission.joints_))
     {
-      ROS_ERROR_STREAM_NAMED("parser","Failed to load joints for transmission "
-        << transmission.name_);
+      ROS_ERROR_STREAM_NAMED("parser","Failed to load joints for transmission '"
+        << transmission.name_ << "'.");
       continue;
     }
 
     // Load actuators
     if(!parseActuators(trans_it, transmission.actuators_))
     {
-      ROS_ERROR_STREAM_NAMED("parser","Failed to load actuators for transmission "
-        << transmission.name_);
+      ROS_ERROR_STREAM_NAMED("parser","Failed to load actuators for transmission '"
+        << transmission.name_ << "'.");
       continue;
     }
 
@@ -109,7 +114,7 @@ bool TransmissionParser::parse(const std::string& urdf, std::vector<Transmission
 
   if( transmissions.empty() )
   {
-    ROS_DEBUG_STREAM_NAMED("parser","No tranmissions found.");
+    ROS_DEBUG_STREAM_NAMED("parser", "No valid tranmissions found.");
   }
 
   return true;
@@ -125,29 +130,56 @@ bool TransmissionParser::parseJoints(TiXmlElement *trans_it, std::vector<JointIn
     // Create new joint
     transmission_interface::JointInfo joint;
 
-    // Joint xml element
-    joint.xml_element_ = joint_it;
-
     // Joint name
     if(joint_it->Attribute("name"))
     {
       joint.name_ = joint_it->Attribute("name");
+      if (joint.name_.empty())
+      {
+        ROS_ERROR_STREAM_NAMED("parser","Empty name attribute specified for joint.");
+        continue;
+      }
     }
     else
     {
-      ROS_ERROR_STREAM_NAMED("parser","No name attribute for joint");
+      ROS_ERROR_STREAM_NAMED("parser","No name attribute specified for joint.");
       return false;
     }
 
-    // \todo: implement other generic joint properties
+    // Hardware interfaces (required)
+    TiXmlElement *hw_iface_it = NULL;
+    for (hw_iface_it = joint_it->FirstChildElement("hardwareInterface"); hw_iface_it;
+         hw_iface_it = hw_iface_it->NextSiblingElement("hardwareInterface"))
+    {
+      if(!hw_iface_it) {continue;}
+      if (!hw_iface_it->GetText())
+      {
+        ROS_DEBUG_STREAM_NAMED("parser","Skipping empty hardware interface element in joint '"
+                               << joint.name_ << "'.");
+        continue;
+      }
+      const std::string hw_iface_name = hw_iface_it->GetText();
+      joint.hardware_interfaces_.push_back(hw_iface_name);
+    }
+    if (joint.hardware_interfaces_.empty())
+    {
+      ROS_ERROR_STREAM_NAMED("parser","No valid hardware interface element found in joint '"
+        << joint.name_ << "'.");
+      continue;
+    }
+
+    // Joint xml element
+    std::stringstream ss;
+    ss << *joint_it;
+    joint.xml_element_ = ss.str();
 
     // Add joint to vector
     joints.push_back(joint);
   }
 
   if(joints.empty())
   {
-    ROS_ERROR_STREAM_NAMED("parser","No joint element found");
+    ROS_DEBUG_NAMED("parser","No valid joint element found.");
     return false;
   }
 
@@ -164,45 +196,79 @@ bool TransmissionParser::parseActuators(TiXmlElement *trans_it, std::vector<Actu
     // Create new actuator
     transmission_interface::ActuatorInfo actuator;
 
-    // Actuator xml element
-    actuator.xml_element_ = actuator_it;
-
     // Actuator name
     if(actuator_it->Attribute("name"))
     {
       actuator.name_ = actuator_it->Attribute("name");
+      if (actuator.name_.empty())
+      {
+        ROS_ERROR_STREAM_NAMED("parser","Empty name attribute specified for actuator.");
+        continue;
+      }
     }
     else
     {
-      ROS_ERROR_STREAM_NAMED("parser","No name attribute for actuator");
+      ROS_ERROR_STREAM_NAMED("parser","No name attribute specified for actuator.");
       return false;
     }
 
-    // Hardware interface
-    TiXmlElement *hardware_interface_child = actuator_it->FirstChildElement("hardwareInterface");
-    if(!hardware_interface_child)
+    // Hardware interfaces (optional)
+    TiXmlElement *hw_iface_it = NULL;
+    for (hw_iface_it = actuator_it->FirstChildElement("hardwareInterface"); hw_iface_it;
+         hw_iface_it = hw_iface_it->NextSiblingElement("hardwareInterface"))
     {
-      ROS_ERROR_STREAM_NAMED("parser","No hardware interface element found for actuator '"
-        << actuator.name_ << "'");
-      continue;
+      if(!hw_iface_it) {continue;}
+      if (!hw_iface_it->GetText())
+      {
+        ROS_DEBUG_STREAM_NAMED("parser","Skipping empty hardware interface element in actuator '"
+                               << actuator.name_ << "'.");
+        continue;
+      }
+      const std::string hw_iface_name = hw_iface_it->GetText();
+      actuator.hardware_interfaces_.push_back(hw_iface_name);
     }
-    actuator.hardware_interface_ = hardware_interface_child->GetText();
-    if(actuator.hardware_interface_.empty())
+    if (actuator.hardware_interfaces_.empty())
     {
-      ROS_ERROR_STREAM_NAMED("parser","No hardware interface string found for actuator '"
-        << actuator.name_ << "'");
-      continue;
+      ROS_DEBUG_STREAM_NAMED("parser","No valid hardware interface element found in actuator '"
+        << actuator.name_ << "'.");
+      // continue; // NOTE: Hardware interface is optional, so we keep on going
     }
 
+//    // Hardware interfaces
+//    TiXmlElement *hw_iface_it = NULL;
+//    for (hw_iface_it = actuator_it->FirstChildElement("hardwareInterface"); hw_iface_it;
+//         hw_iface_it = hw_iface_it->NextSiblingElement("hardwareInterface"))
+//    {
+//      if(!hw_iface_it)
+//      {
+//        ROS_ERROR_STREAM_NAMED("parser","No hardware interface element found for actuator '"
+//          << actuator.name_ << "'.");
+//        continue;
+//      }
+//      const std::string hardware_interface_name = hw_iface_it->GetText();
+//      if (hardware_interface_name.empty())
+//      {
+//        ROS_ERROR_STREAM_NAMED("parser","Skipping empty hardware interface for actuator '"
+//                               << actuator.name_ << "'.");
+//        continue;
+//      }
+//      actuator.hardware_interfaces_.push_back(hardware_interface_name);
+//    }
+
     // \todo: implement other generic actuator properties
 
+    // Actuator xml element
+    std::stringstream ss;
+    ss << *actuator_it;
+    actuator.xml_element_ = ss.str();
+
     // Add actuator to vector
     actuators.push_back(actuator);
   }
 
   if(actuators.empty())
   {
-    ROS_ERROR_STREAM_NAMED("parser","No actuator element found");
+    ROS_DEBUG_NAMED("parser","No valid actuator element found.");
     return false;
   }