update to latests design with optional root link

urdf/include/urdf/model.h

@@ -99,6 +99,11 @@ private:
   /// it's time to build a tree
   bool initTree(std::map<std::string, std::string> &parent_link_tree);
 
+  /// in initXml(), onece tree is built,
+  /// it's time to find the root Link
+  bool initRoot(std::map<std::string, std::string> &parent_link_tree);
+
+
   /// Model is restricted to a tree for now, which means there exists one root link
   ///  typically, root link is the world(inertial).  Where world is a special link
   /// or is the root_link_ the link attached to the world by PLANAR/FLOATING joint?

urdf/src/model.cpp

@@ -238,63 +238,62 @@ bool Model::initXml(TiXmlElement *robot_xml)
     return false;
   }
 
+  // find the root link
+  if (!this->initRoot(parent_link_tree))
+  {
+    ROS_ERROR("failed to find root link");
+    return false;
+  }
+ 
   return true;
 }
 
 bool Model::initTree(std::map<std::string, std::string> &parent_link_tree)
 {
-  this->root_link_.reset();
-
   // loop through all joints, for every link, assign children links and children joints
   for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
   {
     std::string parent_link_name = joint->second->parent_link_name;
     std::string child_link_name = joint->second->child_link_name;
 
-    ROS_DEBUG("    Build tree: joint: '%s' has parent link '%s' and child  link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str());
+    ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child  link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str());
     
-    // case where joint is not connected to any link
     if (parent_link_name.empty() && !child_link_name.empty())
     {
-      ROS_ERROR("    Joint %s specifies only parent link but not child link",(joint->second)->name.c_str());
+      ROS_ERROR("    Joint %s specifies child link but not parent link.",(joint->second)->name.c_str());
       return false;
     }
-    else if (!parent_link_name.empty() &&  child_link_name.empty())
+    else if (!parent_link_name.empty() && child_link_name.empty())
     {
-      ROS_ERROR("    Joint %s specifies only child link but not parent link",(joint->second)->name.c_str());
+      ROS_ERROR("    Joint %s specifies parent link but not child link.",(joint->second)->name.c_str());
       return false;
     }
     else if (parent_link_name.empty() && child_link_name.empty())
     {
-      ROS_DEBUG("    Joint %s has not parent or child link, it is an abstract joint.",(joint->second)->name.c_str());
+      ROS_DEBUG("    Joint %s specifies no parent link and no child link. This is an abstract joint.",(joint->second)->name.c_str());
     }
-
-    // normal joint case
     else
     {
-      // find child and parent link of joint
+      // find child and parent links
       boost::shared_ptr<Link> child_link, parent_link;
       this->getLink(child_link_name, child_link);
-      this->getLink(parent_link_name, parent_link);
       if (!child_link)
       {
-        ROS_ERROR("    Child link '%s' of joint: %s not found",child_link_name.c_str(),joint->first.c_str());
+        ROS_ERROR("    child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() );
         return false;
       }
+      this->getLink(parent_link_name, parent_link);
       if (!parent_link)
       {
-        if (root_link_)
+        ROS_DEBUG("    parent link '%s' of joint '%s' not found. Automatically adding it. This must be the root link", 
-        {
+                  parent_link_name.c_str(), joint->first.c_str() );
-          ROS_ERROR("This tree contains two root links");
+
-          return false; 
-        }
-        ROS_DEBUG("    Parent link '%s' of joint '%s' not found. This should be the root link.", parent_link_name.c_str(), joint->first.c_str() );
         parent_link.reset(new Link);
         parent_link->name = parent_link_name;
-        root_link_ = parent_link;
+        this->links_.insert(make_pair(parent_link->name, parent_link));
-        this->links_.insert(make_pair(parent_link_name, parent_link));
+        ROS_DEBUG("        successfully added new link '%s'", parent_link->name.c_str());
       }
-
+      
       //set parent link for child link
       child_link->setParent(parent_link);
 
@@ -303,28 +302,53 @@ bool Model::initTree(std::map<std::string, std::string> &parent_link_tree)
 
       //set child joint for parent link
       parent_link->addChildJoint(joint->second);
-
+      
       //set child link for parent link
       parent_link->addChild(child_link);
-
+      
       // fill in child/parent string map
       parent_link_tree[child_link->name] = parent_link_name;
-
+      
       ROS_DEBUG("    now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size());
     }
   }
 
-  // we should have found root
-  if (!root_link_){
-    ROS_ERROR("The tree does not contain a root link");
-    return false;
-  }
-
   return true;
 }
 
 
 
+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++)
+  {
+    if (parent_link_tree.find(p->second) == parent_link_tree.end())
+    {
+      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());
+          return false;
+        }
+      }
+      else
+         getLink(p->second,this->root_link_);
+    }
+  }
+  if (!this->root_link_)
+  {
+    ROS_ERROR("No root link found. The robot xml is not a valid tree.");
+    return false;
+  }
+  ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str());
+
+  return true;
+}
 
 boost::shared_ptr<Material> Model::getMaterial(const std::string& name) const
 {

urdf/test/test_robot_model_parser.cpp

@@ -76,7 +76,7 @@ TEST_F(TestParser, test)
     robot_model_xml.LoadFile(g_argv[i]);
     TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
     ASSERT_TRUE(robot_xml != NULL);
-    if (i == g_argc-1)
+    if (i >= g_argc-2)
       ASSERT_TRUE(robot.initXml(robot_xml));
     else
       ASSERT_FALSE(robot.initXml(robot_xml));