From fbd4e7d4d393107b021fed7bc81975a766894b05 Mon Sep 17 00:00:00 2001
From: Wim Meeussen <wim@willowgarage.com>
Date: Mon, 1 Aug 2011 10:19:51 -0700
Subject: [PATCH] Add missing check_urdf tool

---
 urdf/CMakeLists.txt            |  4 ++
 urdf/src/check_urdf.cpp        | 92 ++++++++++++++++++++++++++++++++++
 urdf_parser/src/check_urdf.cpp |  2 +-
 3 files changed, 97 insertions(+), 1 deletion(-)
 create mode 100644 urdf/src/check_urdf.cpp

diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt
index 3dbcdff..921cf65 100644
--- a/urdf/CMakeLists.txt
+++ b/urdf/CMakeLists.txt
@@ -23,7 +23,11 @@ rosbuild_gensrv()
 
 rosbuild_add_library(${PROJECT_NAME} src/model.cpp)
 
+rosbuild_add_executable(check_urdf src/check_urdf.cpp)
+target_link_libraries(check_urdf ${PROJECT_NAME})
+
 rosbuild_add_executable(test_parser EXCLUDE_FROM_ALL test/test_robot_model_parser.cpp)
 target_link_libraries(test_parser ${PROJECT_NAME})
 rosbuild_add_gtest_build_flags(test_parser)
 rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_robot_model_parser.launch)
+
diff --git a/urdf/src/check_urdf.cpp b/urdf/src/check_urdf.cpp
new file mode 100644
index 0000000..7bedf8a
--- /dev/null
+++ b/urdf/src/check_urdf.cpp
@@ -0,0 +1,92 @@
+/*********************************************************************
+* 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:
+* 
+*   * Redistributions 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;
+
+void printTree(boost::shared_ptr<const Link> link,int level = 0)
+{
+  level+=2;
+  int count = 0;
+  for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
+  {
+    if (*child)
+    {
+      for(int j=0;j<level;j++) std::cout << "  "; //indent
+      std::cout << "child(" << (count++)+1 << "):  " << (*child)->name  << std::endl;
+      // first grandchild
+      printTree(*child,level);
+    }
+    else
+    {
+      for(int j=0;j<level;j++) std::cout << " "; //indent
+      std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
+    }
+  }
+
+}
+
+
+int main(int argc, char** argv)
+{
+  if (argc < 2){
+    std::cerr << "Expect URDF or COLLADA xml file to parse" << std::endl;
+    return -1;
+  }
+
+  urdf::Model robot;
+  if (!robot.initFile(argv[1])){
+    std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
+    return -1;
+  }
+  std::cout << "robot name is: " << robot.getName() << std::endl;
+
+  // get info from parser
+  std::cout << "---------- Successfully Parsed XML ---------------" << std::endl;
+  // get root link
+  boost::shared_ptr<const Link> root_link=robot.getRoot();
+  if (!root_link) return -1;
+
+  std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;
+
+  // print entire tree
+  printTree(root_link);
+  return 0;
+}
+
diff --git a/urdf_parser/src/check_urdf.cpp b/urdf_parser/src/check_urdf.cpp
index 46a9e43..3402e49 100644
--- a/urdf_parser/src/check_urdf.cpp
+++ b/urdf_parser/src/check_urdf.cpp
@@ -66,7 +66,7 @@ void printTree(boost::shared_ptr<const Link> link,int level = 0)
 int main(int argc, char** argv)
 {
   if (argc < 2){
-    std::cerr << "Expect xml file to parse" << std::endl;
+    std::cerr << "Expect URDF xml file to parse" << std::endl;
     return -1;
   }