make joint to root mandatory (as in documentation), add visualization tool, and add regression test

urdf/CMakeLists.txt

@@ -27,8 +27,11 @@ rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/model.cpp)
 rosbuild_add_boost_directories()
 #rosbuild_link_boost(${PROJECT_NAME} thread)
 
-rosbuild_add_executable(urdf_check src/urdf_check.cpp)
+rosbuild_add_executable(check_urdf src/check_urdf.cpp)
-target_link_libraries(urdf_check ${PROJECT_NAME})
+target_link_libraries(check_urdf ${PROJECT_NAME})
+
+rosbuild_add_executable(urdf_to_graphiz src/urdf_to_graphiz.cpp)
+target_link_libraries(urdf_to_graphiz ${PROJECT_NAME})
 
 rosbuild_add_executable(mem_test test/memtest.cpp)
 target_link_libraries(mem_test ${PROJECT_NAME})

urdf/include/urdf/model.h

@@ -99,11 +99,6 @@ 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,126 +238,94 @@ 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());
     
-    /// deal with deprecated case
+    // case where joint is not connected to any link
-    if (parent_link_name == "world" && !this->getLink(parent_link_name)){
+    if (parent_link_name.empty() && !child_link_name.empty())
-      ROS_WARN("Joint %s specifies the world link as its parent, but there in no link called world. This used to be allowed, but this behavior has been deprecated. You need to explicitly specify each link in your tree.", joint->first.c_str());
+    {
-      boost::shared_ptr<Link> link;
+      ROS_ERROR("    Joint %s specifies only parent link but not child link",(joint->second)->name.c_str());
-      link.reset(new Link);
+      return false;
-      link->name = "world";
+    }
-      this->links_.insert(make_pair(link->name,link));
+    else if (!parent_link_name.empty() &&  child_link_name.empty())
-      ROS_DEBUG("        successfully added new link '%s'", link->name.c_str());
+    {
+      ROS_ERROR("    Joint %s specifies only child link but not parent 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());
     }
 
-    if (parent_link_name.empty())
+    // normal joint case
-    {
-      ROS_DEBUG("    Joint %s: does not have parent link name specified. Joint is an abstract joint.",(joint->second)->name.c_str());
-    }
-    else if (child_link_name.empty())
-    {
-      ROS_DEBUG("    Joint %s: does not have child link name specified. Joint is an abstract joint.",(joint->second)->name.c_str());
-    }
     else
     {
-      // find parent link
+      // find child and parent link of joint
-      boost::shared_ptr<Link> parent_link;
+      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)
-      if (!parent_link)
       {
-        ROS_ERROR("    parent link '%s' of joint '%s' not found", parent_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;
       }
-      else
+      if (!parent_link)
       {
-        // find child link
+        if (root_link_)
-        boost::shared_ptr<Link> child_link;
-        this->getLink(child_link_name, child_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("This tree contains two root links");
           return false; 
         }
-        else
+        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);
-          //set parent link for child link
+        parent_link->name = parent_link_name;
-          child_link->setParent(parent_link);
+        root_link_ = parent_link;
-
+        this->links_.insert(make_pair(parent_link_name, parent_link));
-          //set parent joint for child link
-          child_link->setParentJoint(joint->second);
-
-          //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());
-        }
       }
+
+      //set parent link for child link
+      child_link->setParent(parent_link);
+
+      //set parent joint for child link
+      child_link->setParentJoint(joint->second);
+
+      //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());
     }
   }
 
-  return true;
+  // we should have found root
-}
+  if (!root_link_){
-
+    ROS_ERROR("The tree does not contain a root link");
-
-
-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
 {
   boost::shared_ptr<Material> ptr;

urdf/src/urdf_to_graphiz.cpp

@@ -0,0 +1,117 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redstributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#include "urdf/model.h"
+#include <iostream>
+#include <fstream>
+
+using namespace urdf;
+using namespace std;
+
+void addChildLinkNames(boost::shared_ptr<const Link> link, ofstream& os)
+{
+  os << "\"" << link->name << "\" [label=\"" << link->name << "\"];" << endl;
+  for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
+    addChildLinkNames(*child, os);
+}
+
+void addChildJointNames(boost::shared_ptr<const Link> link, ofstream& os)
+{
+  double r, p, y;
+  for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
+    (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y);
+    os << "\"" << link->name << "\" -> \"" << (*child)->parent_joint->name 
+       << "\" [label=\"xyz: "
+       << (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " " 
+       << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " " 
+       << (*child)->parent_joint->parent_to_joint_origin_transform.position.z << " " 
+       << "\\nrpy: " << r << " " << p << " " << y << "\"]" << endl;
+    os << "\"" << (*child)->parent_joint->name << "\" -> \"" << (*child)->name << "\"" << endl;
+    addChildJointNames(*child, os);
+  }
+}
+
+
+void printTree(boost::shared_ptr<const Link> link, string file)
+{
+  std::ofstream os;
+  os.open(file.c_str());
+  os << "digraph G {" << endl;
+
+  os << "node [shape=box];" << endl;
+  addChildLinkNames(link, os);
+
+  os << "node [shape=ellipse, color=blue, fontcolor=blue];" << endl;
+  addChildJointNames(link, os);
+
+  os << "}" << endl;
+  os.close();
+}
+
+
+
+int main(int argc, char** argv)
+{
+  if (argc != 2){
+    std::cerr << "Usage: urdf_to_graphiz input.xml" << std::endl;
+    return -1;
+  }
+
+  TiXmlDocument robot_model_xml;
+  robot_model_xml.LoadFile(argv[1]);
+  TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
+  if (!robot_xml){
+    std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
+    return -1;
+  }
+
+  Model robot;
+  if (!robot.initXml(robot_xml)){
+    std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
+    return -1;
+  }
+  string output = robot.getName();
+
+  // print entire tree to file
+  printTree(robot.getRoot(), output+".gv");
+  cout << "Created file " << output << ".gv" << endl;
+
+  string command = "dot -Tpdf "+output+".gv  -o "+output+".pdf";
+  system(command.c_str());
+  cout << "Created file " << output << ".pdf" << endl;
+  return 0;
+}
+

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.xml" />
+  <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" />
 </launch>