Standalone kdl_parser library (#13)

* remove boost dependency

* make catkin and rosconsole optional, replace urdf by urdfdom

* cmake modules for TinyXML and TinyXML2

source: https://github.com/ros/cmake_modules

* append to cmake module path

* replace urdf by urdfdom

* mention source of local cmake module files

* define ROS_WARN and ROS_ERROR as standard error

* remove define guards from header files

* always build shared library

* define ROS_DEBUG as standard output

kdl_parser/CMakeLists.txt

@@ -2,35 +2,68 @@ cmake_minimum_required(VERSION 2.8.3)
 
 project(kdl_parser)
 
-find_package(Boost REQUIRED)
+find_package(catkin QUIET
-include_directories(${Boost_INCLUDE_DIR})
+  COMPONENTS rosconsole cmake_modules
-
-find_package(catkin REQUIRED
-  COMPONENTS rosconsole urdf cmake_modules
 )
+
+if(NOT catkin_FOUND)
+  # use local copies of FindTinyXML.cmake and FindTinyXML2.cmake from
+  # 'cmake_modules' (https://github.com/ros/cmake_modules)
+  list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake)
+endif()
+
+find_package(urdfdom REQUIRED)
+include_directories(${urdfdom_INCLUDE_DIRS})
+
 find_package(orocos_kdl REQUIRED)
 find_package(TinyXML REQUIRED)
 find_package(TinyXML2 REQUIRED)
 
-include_directories(include ${orocos_kdl_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS} ${TinyXML2_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
+# check for rosconsole
+# We check additionally for catkin to distinguish between an "official" ROS distribution
+# and the one provided in the distribution's repository.
+find_package(rosconsole QUIET)
+if(rosconsole_FOUND AND catkin_FOUND)
+  add_definitions(-DHAS_ROS)
+endif()
 
-link_directories(${catkin_LIBRARY_DIRS})
+find_package(urdf QUIET)
-link_directories(${Boost_LIBRARY_DIRS})
+if(urdf_FOUND)
+  add_definitions(-DHAS_URDF)
+  include_directories(${urdf_INCLUDE_DIRS})
+endif()
+
+include_directories(include ${orocos_kdl_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS} ${TinyXML2_INCLUDE_DIRS})
 
 add_compile_options(-std=c++11)
 
+if(catkin_FOUND)
+  link_directories(${catkin_LIBRARY_DIRS})
+  include_directories(${catkin_INCLUDE_DIRS})
+
   catkin_package(
     LIBRARIES ${PROJECT_NAME} ${orocos_kdl_LIBRARIES}
     INCLUDE_DIRS include
     CATKIN_DEPENDS rosconsole urdf
     DEPENDS orocos_kdl TinyXML TinyXML2
   )
+endif()
 
-add_library(${PROJECT_NAME} src/kdl_parser.cpp)
+add_library(${PROJECT_NAME} SHARED src/kdl_parser.cpp)
 target_link_libraries(${PROJECT_NAME}
-  ${TinyXML_LIBRARIES} ${TinyXML2_LIBRARIES} ${orocos_kdl_LIBRARIES} ${catkin_LIBRARIES}
+  ${TinyXML_LIBRARIES} ${TinyXML2_LIBRARIES} ${orocos_kdl_LIBRARIES}
 )
 
+target_link_libraries(${PROJECT_NAME} ${urdfdom_LIBRARIES})
+
+if(catkin_FOUND)
+  target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+endif()
+
+if(urdf_FOUND)
+  target_link_libraries(${PROJECT_NAME} ${urdf_LIBRARIES})
+endif()
+
 if(WIN32)
   target_compile_definitions(${PROJECT_NAME} PRIVATE "KDL_PARSER_BUILDING_DLL")
 endif()
@@ -38,7 +71,7 @@ endif()
 add_executable(check_kdl_parser src/check_kdl_parser.cpp )
 target_link_libraries(check_kdl_parser ${PROJECT_NAME})
 
-if(CATKIN_ENABLE_TESTING)
+if(catkin_FOUND AND CATKIN_ENABLE_TESTING)
   find_package(catkin REQUIRED COMPONENTS roscpp rostest)
   add_rostest_gtest(test_kdl_parser test/test_kdl_parser.launch test/test_kdl_parser.cpp )
   target_link_libraries(test_kdl_parser ${PROJECT_NAME})
@@ -47,6 +80,7 @@ if(CATKIN_ENABLE_TESTING)
   target_link_libraries(test_inertia_rpy ${PROJECT_NAME})
 endif()
 
+if(catkin_FOUND)
   # How does CATKIN do this?
   #rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_kdl_parser.launch)
   install(TARGETS ${PROJECT_NAME}
@@ -54,3 +88,10 @@ install(TARGETS ${PROJECT_NAME}
 
   install(DIRECTORY include/${PROJECT_NAME}/
     DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+else()
+  install(TARGETS ${PROJECT_NAME}
+          LIBRARY DESTINATION lib
+          ARCHIVE DESTINATION lib)
+  install(DIRECTORY include/${PROJECT_NAME}/
+          DESTINATION include)
+endif()

kdl_parser/cmake/FindTinyXML.cmake

@@ -0,0 +1,74 @@
+##################################################################################################
+# 
+# CMake script for finding TinyXML.
+# 
+# Input variables:
+# 
+# - TinyXML_ROOT_DIR (optional): When specified, header files and libraries will be searched for in
+#     ${TinyXML_ROOT_DIR}/include
+#     ${TinyXML_ROOT_DIR}/libs
+#   respectively, and the default CMake search order will be ignored. When unspecified, the default
+#   CMake search order is used.
+#   This variable can be specified either as a CMake or environment variable. If both are set,
+#   preference is given to the CMake variable.
+#   Use this variable for finding packages installed in a nonstandard location, or for enforcing
+#   that one of multiple package installations is picked up.
+# 
+# 
+# Cache variables (not intended to be used in CMakeLists.txt files)
+# 
+# - TinyXML_INCLUDE_DIR: Absolute path to package headers.
+# - TinyXML_LIBRARY: Absolute path to library.
+# 
+# 
+# Output variables:
+# 
+# - TinyXML_FOUND: Boolean that indicates if the package was found
+# - TinyXML_INCLUDE_DIRS: Paths to the necessary header files
+# - TinyXML_LIBRARIES: Package libraries
+# 
+# 
+# Example usage:
+# 
+#  find_package(TinyXML)
+#  if(NOT TinyXML_FOUND)
+#    # Error handling
+#  endif()
+#  ...
+#  include_directories(${TinyXML_INCLUDE_DIRS} ...)
+#  ...
+#  target_link_libraries(my_target ${TinyXML_LIBRARIES})
+# 
+##################################################################################################
+
+# Get package location hint from environment variable (if any)
+if(NOT TinyXML_ROOT_DIR AND DEFINED ENV{TinyXML_ROOT_DIR})
+  set(TinyXML_ROOT_DIR "$ENV{TinyXML_ROOT_DIR}" CACHE PATH
+      "TinyXML base directory location (optional, used for nonstandard installation paths)")
+endif()
+
+# Search path for nonstandard package locations
+if(TinyXML_ROOT_DIR)
+  set(TinyXML_INCLUDE_PATH PATHS "${TinyXML_ROOT_DIR}/include" NO_DEFAULT_PATH)
+  set(TinyXML_LIBRARY_PATH PATHS "${TinyXML_ROOT_DIR}/lib"     NO_DEFAULT_PATH)
+endif()
+
+# Find headers and libraries
+find_path(TinyXML_INCLUDE_DIR NAMES tinyxml.h PATH_SUFFIXES "tinyxml" ${TinyXML_INCLUDE_PATH})
+find_library(TinyXML_LIBRARY  NAMES tinyxml   PATH_SUFFIXES "tinyxml" ${TinyXML_LIBRARY_PATH})
+
+mark_as_advanced(TinyXML_INCLUDE_DIR
+                 TinyXML_LIBRARY)
+
+# Output variables generation
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(TinyXML DEFAULT_MSG TinyXML_LIBRARY
+                                                      TinyXML_INCLUDE_DIR)
+
+set(TinyXML_FOUND ${TINYXML_FOUND}) # Enforce case-correctness: Set appropriately cased variable...
+unset(TINYXML_FOUND) # ...and unset uppercase variable generated by find_package_handle_standard_args
+
+if(TinyXML_FOUND)
+  set(TinyXML_INCLUDE_DIRS ${TinyXML_INCLUDE_DIR})
+  set(TinyXML_LIBRARIES ${TinyXML_LIBRARY})
+endif()

kdl_parser/cmake/FindTinyXML2.cmake

@@ -0,0 +1,74 @@
+##################################################################################################
+# 
+# CMake script for finding TinyXML2.
+# 
+# Input variables:
+# 
+# - TinyXML2_ROOT_DIR (optional): When specified, header files and libraries will be searched for in
+#     ${TinyXML2_ROOT_DIR}/include
+#     ${TinyXML2_ROOT_DIR}/libs
+#   respectively, and the default CMake search order will be ignored. When unspecified, the default
+#   CMake search order is used.
+#   This variable can be specified either as a CMake or environment variable. If both are set,
+#   preference is given to the CMake variable.
+#   Use this variable for finding packages installed in a nonstandard location, or for enforcing
+#   that one of multiple package installations is picked up.
+# 
+# 
+# Cache variables (not intended to be used in CMakeLists.txt files)
+# 
+# - TinyXML2_INCLUDE_DIR: Absolute path to package headers.
+# - TinyXML2_LIBRARY: Absolute path to library.
+# 
+# 
+# Output variables:
+# 
+# - TinyXML2_FOUND: Boolean that indicates if the package was found
+# - TinyXML2_INCLUDE_DIRS: Paths to the necessary header files
+# - TinyXML2_LIBRARIES: Package libraries
+# 
+# 
+# Example usage:
+# 
+#  find_package(TinyXML2)
+#  if(NOT TinyXML2_FOUND)
+#    # Error handling
+#  endif()
+#  ...
+#  include_directories(${TinyXML2_INCLUDE_DIRS} ...)
+#  ...
+#  target_link_libraries(my_target ${TinyXML2_LIBRARIES})
+# 
+##################################################################################################
+
+# Get package location hint from environment variable (if any)
+if(NOT TinyXML2_ROOT_DIR AND DEFINED ENV{TinyXML2_ROOT_DIR})
+  set(TinyXML2_ROOT_DIR "$ENV{TinyXML2_ROOT_DIR}" CACHE PATH
+      "TinyXML2 base directory location (optional, used for nonstandard installation paths)")
+endif()
+
+# Search path for nonstandard package locations
+if(TinyXML2_ROOT_DIR)
+  set(TinyXML2_INCLUDE_PATH PATHS "${TinyXML2_ROOT_DIR}/include" NO_DEFAULT_PATH)
+  set(TinyXML2_LIBRARY_PATH PATHS "${TinyXML2_ROOT_DIR}/lib"     NO_DEFAULT_PATH)
+endif()
+
+# Find headers and libraries
+find_path(TinyXML2_INCLUDE_DIR NAMES tinyxml2.h PATH_SUFFIXES "tinyxml2" ${TinyXML2_INCLUDE_PATH})
+find_library(TinyXML2_LIBRARY  NAMES tinyxml2   PATH_SUFFIXES "tinyxml2" ${TinyXML2_LIBRARY_PATH})
+
+mark_as_advanced(TinyXML2_INCLUDE_DIR
+                 TinyXML2_LIBRARY)
+
+# Output variables generation
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(TinyXML2 DEFAULT_MSG TinyXML2_LIBRARY
+                                                       TinyXML2_INCLUDE_DIR)
+
+set(TinyXML2_FOUND ${TINYXML2_FOUND}) # Enforce case-correctness: Set appropriately cased variable...
+unset(TINYXML2_FOUND) # ...and unset uppercase variable generated by find_package_handle_standard_args
+
+if(TinyXML2_FOUND)
+  set(TinyXML2_INCLUDE_DIRS ${TinyXML2_INCLUDE_DIR})
+  set(TinyXML2_LIBRARIES ${TinyXML2_LIBRARY})
+endif()

kdl_parser/include/kdl_parser/kdl_parser.hpp

@@ -59,7 +59,7 @@ bool treeFromFile(const std::string & file, KDL::Tree & tree);
 /** Constructs a KDL tree from the parameter server, given the parameter name
  * \param param the name of the parameter on the parameter server
  * \param tree The resulting KDL Tree
- * returns true on success, false on failure
+ * returns true on success, false on failure or if built without ROS
  */
 KDL_PARSER_PUBLIC
 bool treeFromParam(const std::string & param, KDL::Tree & tree);

kdl_parser/src/check_kdl_parser.cpp

@@ -40,7 +40,8 @@
 #include "kdl_parser/kdl_parser.hpp"
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/frames_io.hpp>
-#include <urdf/model.h>
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
 
 void printLink(const KDL::SegmentMap::const_iterator & link, const std::string & prefix)
 {
@@ -58,14 +59,14 @@ int main(int argc, char ** argv)
     std::cerr << "Expect xml file to parse" << std::endl;
     return -1;
   }
-  urdf::Model robot_model;
+  urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFile(argv[1]);
-  if (!robot_model.initFile(argv[1])) {
+  if (!robot_model) {
     std::cerr << "Could not generate robot model" << std::endl;
     return false;
   }
 
   KDL::Tree my_tree;
-  if (!kdl_parser::treeFromUrdfModel(robot_model, my_tree)) {
+  if (!kdl_parser::treeFromUrdfModel(*robot_model, my_tree)) {
     std::cerr << "Could not extract kdl tree" << std::endl;
     return false;
   }

kdl_parser/src/kdl_parser.cpp

@@ -39,10 +39,24 @@
 #include <string>
 #include <vector>
 
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <kdl/frames_io.hpp>
+
+#ifdef HAS_ROS
+#include <ros/console.h>
+#else
+// forward ROS warnings and errors to stderr
+#define ROS_DEBUG(...) fprintf(stdout, __VA_ARGS__);
+#define ROS_ERROR(...) fprintf(stderr, __VA_ARGS__);
+#define ROS_WARN(...) fprintf(stderr, __VA_ARGS__);
+#endif
+
+#ifdef HAS_URDF
 #include <urdf/model.h>
 #include <urdf/urdfdom_compatibility.h>
-#include <kdl/frames_io.hpp>
+#endif
-#include <ros/console.h>
 
 namespace kdl_parser
 {
@@ -86,8 +100,7 @@ KDL::Joint toKdl(urdf::JointSharedPtr jnt)
         return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::TransAxis);
       }
     default: {
-        ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint",
+        ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
-          jnt->name.c_str());
         return KDL::Joint(jnt->name, KDL::Joint::None);
       }
   }
@@ -156,39 +169,49 @@ bool addChildrenToTree(urdf::LinkConstSharedPtr root, KDL::Tree & tree)
   return true;
 }
 
-
 bool treeFromFile(const std::string & file, KDL::Tree & tree)
 {
-  tinyxml2::XMLDocument urdf_xml;
+  const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFile(file);
-  urdf_xml.LoadFile(file.c_str());
+  return kdl_parser::treeFromUrdfModel(*robot_model, tree);
-  return treeFromXml(&urdf_xml, tree);
 }
 
+
 bool treeFromParam(const std::string & param, KDL::Tree & tree)
 {
+#if defined(HAS_ROS) && defined(HAS_URDF)
   urdf::Model robot_model;
   if (!robot_model.initParam(param)){
     ROS_ERROR("Could not generate robot model");
     return false;
   }
   return treeFromUrdfModel(robot_model, tree);
+#else
+  return false;
+#endif
 }
 
+
 bool treeFromString(const std::string & xml, KDL::Tree & tree)
 {
-  tinyxml2::XMLDocument urdf_xml;
+  const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDF(xml);
-  urdf_xml.Parse(xml.c_str());
+  if (!robot_model) {
-  return treeFromXml(&urdf_xml, tree);
+    ROS_ERROR("Could not generate robot model");
+    return false;
+  }
+  return kdl_parser::treeFromUrdfModel(*robot_model, tree);
 }
 
 bool treeFromXml(const tinyxml2::XMLDocument * xml_doc, KDL::Tree & tree)
 {
-  urdf::Model robot_model;
+  if (!xml_doc) {
-  if (!robot_model.initXml(xml_doc)) {
+    ROS_ERROR("Could not parse the xml document");
-    ROS_ERROR("Could not generate robot model");
     return false;
   }
-  return treeFromUrdfModel(robot_model, tree);
+
+  tinyxml2::XMLPrinter printer;
+  xml_doc->Print(&printer);
+
+  return treeFromString(printer.CStr(), tree);
 }
 
 bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree)
@@ -198,14 +221,10 @@ bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree)
     return false;
   }
 
-  urdf::Model robot_model;
   std::stringstream ss;
   ss << *xml_doc;
-  if (!robot_model.initString(ss.str())) {
+
-    ROS_ERROR("Could not generate robot model");
+  return treeFromString(ss.str(), tree);
-    return false;
-  }
-  return treeFromUrdfModel(robot_model, tree);
 }
 
 bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree)