fix tests for multiple trees, and clean up regression tests

urdf/src/model.cpp

@@ -232,12 +232,6 @@ bool Model::initXml(TiXmlElement *robot_xml)
     return false;
   }
 
-  // make sure tree is not empty
-  if (parent_link_tree.empty()){
-    ROS_ERROR("The robot xml does not contain any valid links. Are you parsing an empty file, or an un-processed xacro file?");
-    return false;
-  }
-
   // find the root link
   if (!this->initRoot(parent_link_tree))
   {
@@ -323,22 +317,24 @@ bool Model::initRoot(std::map<std::string, std::string> &parent_link_tree)
 
   this->root_link_.reset();
 
-  for (std::map<std::string, std::string>::iterator p=parent_link_tree.begin(); p!=parent_link_tree.end(); p++)
+  //  for (std::map<std::string, std::string>::iterator p=parent_link_tree.begin(); p!=parent_link_tree.end(); p++)
+  // find the links that have no parent in the tree
+  for (std::map<std::string, boost::shared_ptr<Link> >::iterator l=this->links_.begin(); l!=this->links_.end(); l++)  
   {
-    if (parent_link_tree.find(p->second) == parent_link_tree.end())
+    std::map<std::string, std::string >::iterator parent = parent_link_tree.find(l->first);
+    if (parent == parent_link_tree.end())
     {
-      if (this->root_link_)
+      // store root link
+      if (!this->root_link_)
       {
-        ROS_DEBUG("child '%s', parent '%s', root '%s'", p->first.c_str(), p->second.c_str(), this->root_link_->name.c_str());
-        if (this->root_link_->name != p->second)
-        {
-          ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), p->second.c_str());
+         getLink(l->first, this->root_link_);
+      }
+      // we already found a root link
+      else{
+        ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str());
         return false;
       }
     }
-      else
-         getLink(p->second,this->root_link_);
-    }
   }
   if (!this->root_link_)
   {

urdf/test/fail_pr2_desc_no_joint2.urdf

@@ -10,6 +10,7 @@
     <parent link="base_link"//>
     <child link="fl_caster_rotation_link"//>
   </joint>
+
   <link name="fl_caster_rotation_link">
     <inertial>
       <mass value="3.473082"/>

urdf/test/fail_three_links_one_joint.urdf

@@ -0,0 +1,25 @@
+<?xml verison="1.0"?>
+<robot name="no_visual">
+  <link name="link1">
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+  <link name="link3">
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+  <link name="link2">
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
+    </inertial>
+  </link>
+  <joint name="joint12" type="fixed">
+    <parent link="link1"/>
+    <child link="link2"/>
+  </link>
+</robot>

urdf/test/pr2_desc_no_joint.urdf

@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 
 <robot name="pr2">
-    <joint name="fl_caster_rotation_joint"/>
+    <joint name="fl_caster_rotation_joint" type="continuous"/>
     <link name="fl_caster_rotation_link">
       <inertial>
         <mass value="3.473082"/>

urdf/test/test_robot_model_parser.cpp

@@ -70,16 +70,16 @@ protected:
 
 TEST_F(TestParser, test)
 {
-  for (int i=1; i<g_argc; i++){
-    printf("\n\nprocessing file %d : %s\n",i,g_argv[i]);
-    TiXmlDocument robot_model_xml;
-    robot_model_xml.LoadFile(g_argv[i]);
-    TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
-    ASSERT_TRUE(robot_xml != NULL);
-    if (i >= g_argc-2)
-      ASSERT_TRUE(robot.initXml(robot_xml));
+  std::string folder = std::string(g_argv[1]) + "/test/";
+  printf("Folder %s",folder.c_str());
+  for (int i=2; i<g_argc; i++){
+    std::string file = g_argv[i];
+    bool expect_success = (file.substr(0,5)  != "fail_");
+    ROS_ERROR("Parsing file %d/%d called %s, expecting %d",(i-1), g_argc-1, (folder + file).c_str(), expect_success);
+    if (!expect_success)
+      ASSERT_FALSE(robot.initFile(folder + file));
     else
-      ASSERT_FALSE(robot.initXml(robot_xml));
+      ASSERT_TRUE(robot.initFile(folder + file));
   }
   SUCCEED();
 }

urdf/test/test_robot_model_parser.launch

@@ -1,3 +1,3 @@
 <launch>
-  <test test-name="robot_model_parser_test" pkg="urdf" type="test_parser" args="$(find urdf)/test/pr2_desc_double_joint.xml $(find urdf)/test/pr2_desc_loop.xml $(find urdf)/test/pr2_desc_no_joint.xml $(find urdf)/test/pr2_desc_two_trees.xml $(find urdf)/test/pr2_desc_bracket.xml  $(find urdf)/test/pr2_desc_double.xml $(find urdf)/test/pr2_desc_no_joint2.xml $(find urdf)/test/pr2_desc_parent_itself.xml $(find urdf)/test/pr2_desc_no_filename_in_mesh.xml $(find urdf)/test/pr2_desc_explicit_world.xml $(find urdf)/test/pr2_desc.xml" />
+  <test test-name="robot_model_parser_test" pkg="urdf" type="test_parser" args="$(find urdf) fail_pr2_desc_bracket.urdf fail_pr2_desc_double_joint.urdf fail_pr2_desc_double.urdf fail_pr2_desc_loop.urdf fail_pr2_desc_no_filename_in_mesh.urdf fail_pr2_desc_no_joint2.urdf fail_pr2_desc_parent_itself.urdf fail_pr2_desc_two_trees.urdf fail_three_links_one_joint.urdf no_visual.urdf one_link.urdf pr2_desc_explicit_world.urdf pr2_desc_no_joint.urdf pr2_desc.urdf two_links_one_joint.urdf" />
 </launch>