Standalone kdl_parser library

kdl_parser/CMakeLists.txt

@@ -2,43 +2,71 @@ cmake_minimum_required(VERSION 2.8.3)
 
 project(kdl_parser)
 
-find_package(Boost REQUIRED)
-include_directories(${Boost_INCLUDE_DIR})
-
-find_package(catkin REQUIRED
-  COMPONENTS rosconsole urdf cmake_modules
+find_package(catkin QUIET
+  COMPONENTS rosconsole cmake_modules
 )
+
+if(NOT catkin_FOUND)
+  set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
+endif()
+
+find_package(urdf QUIET)
+if(urdf_FOUND)
+  add_definitions(-DHAS_URDF)
+  include_directories(${urdf_INCLUDE_DIRS})
+else()
+  find_package(urdfdom REQUIRED)
+  include_directories(${urdfdom_INCLUDE_DIRS})
+endif()
+
 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})
+find_package(rosconsole QUIET)
+if(rosconsole_FOUND)
+  add_definitions(-DHAS_ROS)
+endif()
 
-link_directories(${catkin_LIBRARY_DIRS})
-link_directories(${Boost_LIBRARY_DIRS})
+include_directories(include ${orocos_kdl_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS} ${TinyXML2_INCLUDE_DIRS})
 
 add_compile_options(-std=c++11)
 
-catkin_package(
-  LIBRARIES ${PROJECT_NAME} ${orocos_kdl_LIBRARIES}
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS rosconsole urdf
-  DEPENDS orocos_kdl TinyXML TinyXML2
-)
+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)
 target_link_libraries(${PROJECT_NAME}
-  ${TinyXML_LIBRARIES} ${TinyXML2_LIBRARIES} ${orocos_kdl_LIBRARIES} ${catkin_LIBRARIES}
+  ${TinyXML_LIBRARIES} ${TinyXML2_LIBRARIES} ${orocos_kdl_LIBRARIES}
 )
 
+if(urdf_FOUND)
+  target_link_libraries(${PROJECT_NAME} ${urdf_LIBRARIES})
+else()
+  target_link_libraries(${PROJECT_NAME} ${urdfdom_LIBRARIES})
+endif()
+
+if(catkin_FOUND)
+  target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+endif()
+
 if(WIN32)
   target_compile_definitions(${PROJECT_NAME} PRIVATE "KDL_PARSER_BUILDING_DLL")
 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,10 +75,18 @@ if(CATKIN_ENABLE_TESTING)
   target_link_libraries(test_inertia_rpy ${PROJECT_NAME})
 endif()
 
-# How does CATKIN do this?
-#rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_kdl_parser.launch)
-install(TARGETS ${PROJECT_NAME}
-  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+if(catkin_FOUND)
+  # How does CATKIN do this?
+  #rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_kdl_parser.launch)
+  install(TARGETS ${PROJECT_NAME}
+    DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
 
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+  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

@@ -61,8 +61,10 @@ bool treeFromFile(const std::string & file, KDL::Tree & tree);
  * \param tree The resulting KDL Tree
  * returns true on success, false on failure
  */
+#ifdef HAS_URDF
 KDL_PARSER_PUBLIC
 bool treeFromParam(const std::string & param, KDL::Tree & tree);
+#endif
 
 /** Constructs a KDL tree from a string containing xml
  * \param xml A string containing the xml description of the robot
@@ -77,8 +79,10 @@ bool treeFromString(const std::string & xml, KDL::Tree & tree);
  * \param[out] tree The resulting KDL Tree
  * \return true on success, false on failure
  */
+#ifdef HAS_URDF
 KDL_PARSER_PUBLIC
 bool treeFromXml(const tinyxml2::XMLDocument * xml_doc, KDL::Tree & tree);
+#endif
 
 /** Constructs a KDL tree from a TinyXML document
  * \param[in] xml_doc The document containing the xml description of the robot

kdl_parser/src/check_kdl_parser.cpp

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