From 4454445f4e106d9e1ca9651f2e0bc50d68c6c512 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 19 Mar 2019 14:30:15 +0000 Subject: [PATCH] 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 | 89 ++++++++++++++------ kdl_parser/cmake/FindTinyXML.cmake | 74 ++++++++++++++++ kdl_parser/cmake/FindTinyXML2.cmake | 74 ++++++++++++++++ kdl_parser/include/kdl_parser/kdl_parser.hpp | 2 +- kdl_parser/src/check_kdl_parser.cpp | 9 +- kdl_parser/src/kdl_parser.cpp | 61 +++++++++----- 6 files changed, 259 insertions(+), 50 deletions(-) create mode 100644 kdl_parser/cmake/FindTinyXML.cmake create mode 100644 kdl_parser/cmake/FindTinyXML2.cmake diff --git a/kdl_parser/CMakeLists.txt b/kdl_parser/CMakeLists.txt index cc67e9b..f4da26b 100644 --- a/kdl_parser/CMakeLists.txt +++ b/kdl_parser/CMakeLists.txt @@ -2,34 +2,67 @@ 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) + # 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}) -link_directories(${Boost_LIBRARY_DIRS}) +find_package(urdf QUIET) +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) -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} SHARED src/kdl_parser.cpp) +target_link_libraries(${PROJECT_NAME} + ${TinyXML_LIBRARIES} ${TinyXML2_LIBRARIES} ${orocos_kdl_LIBRARIES} ) -add_library(${PROJECT_NAME} src/kdl_parser.cpp) -target_link_libraries(${PROJECT_NAME} - ${TinyXML_LIBRARIES} ${TinyXML2_LIBRARIES} ${orocos_kdl_LIBRARIES} ${catkin_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") @@ -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,10 +80,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() diff --git a/kdl_parser/cmake/FindTinyXML.cmake b/kdl_parser/cmake/FindTinyXML.cmake new file mode 100644 index 0000000..aabb323 --- /dev/null +++ b/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() diff --git a/kdl_parser/cmake/FindTinyXML2.cmake b/kdl_parser/cmake/FindTinyXML2.cmake new file mode 100644 index 0000000..87ff2fb --- /dev/null +++ b/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() diff --git a/kdl_parser/include/kdl_parser/kdl_parser.hpp b/kdl_parser/include/kdl_parser/kdl_parser.hpp index f56e5f1..f6daafd 100644 --- a/kdl_parser/include/kdl_parser/kdl_parser.hpp +++ b/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); diff --git a/kdl_parser/src/check_kdl_parser.cpp b/kdl_parser/src/check_kdl_parser.cpp index bb4cfb1..5025402 100644 --- a/kdl_parser/src/check_kdl_parser.cpp +++ b/kdl_parser/src/check_kdl_parser.cpp @@ -40,7 +40,8 @@ #include "kdl_parser/kdl_parser.hpp" #include #include -#include +#include +#include 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; - if (!robot_model.initFile(argv[1])) { + urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFile(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; } diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp index b69d6c7..d98e8e4 100644 --- a/kdl_parser/src/kdl_parser.cpp +++ b/kdl_parser/src/kdl_parser.cpp @@ -39,10 +39,24 @@ #include #include +#include +#include + +#include + +#ifdef HAS_ROS +#include +#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 #include -#include -#include +#endif 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", - jnt->name.c_str()); + ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", 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; - urdf_xml.LoadFile(file.c_str()); - return treeFromXml(&urdf_xml, tree); + const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFile(file); + return kdl_parser::treeFromUrdfModel(*robot_model, 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; - urdf_xml.Parse(xml.c_str()); - return treeFromXml(&urdf_xml, tree); + const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDF(xml); + if (!robot_model) { + 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 (!robot_model.initXml(xml_doc)) { - ROS_ERROR("Could not generate robot model"); + if (!xml_doc) { + ROS_ERROR("Could not parse the xml document"); 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 false; - } - return treeFromUrdfModel(robot_model, tree); + + return treeFromString(ss.str(), tree); } bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree)