bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Christian Rauch 2019-03-19 14:30:15 +00:00 committed by Shane Loretz
parent 30c95633d8
commit 4454445f4e
6 changed files with 259 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
kdl_parser/CMakeLists.txt
View File

@ -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()

74
kdl_parser/cmake/FindTinyXML.cmake Normal file
View File

@ -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()

74
kdl_parser/cmake/FindTinyXML2.cmake Normal file
View File

@ -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()

2
kdl_parser/include/kdl_parser/kdl_parser.hpp
View File

@ -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);

9
kdl_parser/src/check_kdl_parser.cpp
View File

@ -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;
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;
}

61
kdl_parser/src/kdl_parser.cpp
View File

@ -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>
#include <ros/console.h>
#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)