From b71646d19e27592ed140237a818e0bf525f7ef03 Mon Sep 17 00:00:00 2001 From: Dan Rose Date: Mon, 27 Apr 2020 18:17:31 -0500 Subject: [PATCH] Switch from tinyxml -> tinyxml2 --- .travis.yml | 2 +- CMakeLists.txt | 6 +- cmake/FindTinyXML.cmake | 74 --------------- cmake/urdfdom-config.cmake.in | 2 +- package.xml | 8 +- urdf_parser/CMakeLists.txt | 8 +- urdf_parser/include/urdf_parser/urdf_parser.h | 7 +- urdf_parser/src/joint.cpp | 80 ++++++++-------- urdf_parser/src/link.cpp | 94 ++++++++----------- urdf_parser/src/model.cpp | 16 ++-- urdf_parser/src/pose.cpp | 15 ++- urdf_parser/src/twist.cpp | 3 +- urdf_parser/src/urdf_model_state.cpp | 2 +- urdf_parser/src/urdf_sensor.cpp | 2 +- urdf_parser/src/world.cpp | 18 ++-- urdf_parser/src/xml_helpers.cpp | 41 ++++++++ urdf_parser/src/xml_helpers.h | 43 +++++++++ 17 files changed, 205 insertions(+), 216 deletions(-) delete mode 100644 cmake/FindTinyXML.cmake create mode 100644 urdf_parser/src/xml_helpers.cpp create mode 100644 urdf_parser/src/xml_helpers.h diff --git a/.travis.yml b/.travis.yml index c711dc97..b06a971f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,7 +8,7 @@ compiler: script: "./.travis/build" before_install: - sudo apt-get update -qq - - sudo apt-get install -qq libtinyxml-dev + - sudo apt-get install -qq libtinyxml2-dev matrix: allow_failures: - compiler: clang diff --git a/CMakeLists.txt b/CMakeLists.txt index 863f0447..5e16154a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,9 +32,9 @@ endif (MSVC OR MSVC90 OR MSVC10) set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") -find_package(tinyxml_vendor REQUIRED) -find_package(TinyXML REQUIRED) -include_directories(SYSTEM ${TinyXML_INCLUDE_DIRS}) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) +include_directories(SYSTEM ${TinyXML2_INCLUDE_DIRS}) find_package(urdfdom_headers 1.0 REQUIRED) include_directories(SYSTEM ${urdfdom_headers_INCLUDE_DIRS}) diff --git a/cmake/FindTinyXML.cmake b/cmake/FindTinyXML.cmake deleted file mode 100644 index aabb323d..00000000 --- a/cmake/FindTinyXML.cmake +++ /dev/null @@ -1,74 +0,0 @@ -################################################################################################## -# -# 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/cmake/urdfdom-config.cmake.in b/cmake/urdfdom-config.cmake.in index 3a0916df..3cb08327 100644 --- a/cmake/urdfdom-config.cmake.in +++ b/cmake/urdfdom-config.cmake.in @@ -3,7 +3,7 @@ if (@PKG_NAME@_CONFIG_INCLUDED) endif() set(@PKG_NAME@_CONFIG_INCLUDED TRUE) -set(@PKG_NAME@_INCLUDE_DIRS "${@PROJECT_NAME@_DIR}/@RELATIVE_PATH_CMAKE_DIR_TO_PREFIX@/include" "@TinyXML_INCLUDE_DIRS@") +set(@PKG_NAME@_INCLUDE_DIRS "${@PROJECT_NAME@_DIR}/@RELATIVE_PATH_CMAKE_DIR_TO_PREFIX@/include" "@TinyXML2_INCLUDE_DIRS@") foreach(lib @PKG_LIBRARIES@) set(onelib "${lib}-NOTFOUND") diff --git a/package.xml b/package.xml index ef794467..7fdf2c5c 100644 --- a/package.xml +++ b/package.xml @@ -13,16 +13,16 @@ console_bridge_vendor libconsole-bridge-dev - tinyxml - tinyxml_vendor + tinyxml2 + tinyxml2_vendor urdfdom_headers cmake console_bridge_vendor libconsole-bridge-dev - tinyxml - tinyxml_vendor + tinyxml2 + tinyxml2_vendor urdfdom_headers diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt index 1118de16..f5629e0b 100644 --- a/urdf_parser/CMakeLists.txt +++ b/urdf_parser/CMakeLists.txt @@ -1,25 +1,25 @@ -add_library(urdfdom_world SHARED src/pose.cpp src/model.cpp src/link.cpp src/joint.cpp src/world.cpp) +add_library(urdfdom_world SHARED src/pose.cpp src/model.cpp src/link.cpp src/joint.cpp src/world.cpp src/xml_helpers.cpp) target_include_directories(urdfdom_world PUBLIC "$" "$") target_link_libraries(urdfdom_world ${TinyXML_LIBRARIES} ${console_bridge_LIBRARIES}) set_target_properties(urdfdom_world PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) -add_library(urdfdom_model SHARED src/pose.cpp src/model.cpp src/link.cpp src/joint.cpp) +add_library(urdfdom_model SHARED src/pose.cpp src/model.cpp src/link.cpp src/joint.cpp src/xml_helpers.cpp) target_include_directories(urdfdom_model PUBLIC "$" "$") target_link_libraries(urdfdom_model ${TinyXML_LIBRARIES} ${console_bridge_LIBRARIES}) set_target_properties(urdfdom_model PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) -add_library(urdfdom_sensor SHARED src/urdf_sensor.cpp) +add_library(urdfdom_sensor SHARED src/urdf_sensor.cpp src/xml_helpers.cpp) target_include_directories(urdfdom_sensor PUBLIC "$" "$") target_link_libraries(urdfdom_sensor urdfdom_model ${TinyXML_LIBRARIES} ${console_bridge_LIBRARIES}) set_target_properties(urdfdom_sensor PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) -add_library(urdfdom_model_state SHARED src/urdf_model_state.cpp src/twist.cpp) +add_library(urdfdom_model_state SHARED src/urdf_model_state.cpp src/twist.cpp src/xml_helpers.cpp) target_include_directories(urdfdom_model_state PUBLIC "$" "$") diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index a2246e79..de2b79fb 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include #include @@ -48,18 +48,19 @@ #include "exportdecl.h" +using TiXmlDocument = tinyxml2::XMLDocument; +using TiXmlElement = tinyxml2::XMLElement; + namespace urdf_export_helpers { URDFDOM_DLLAPI std::string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL); URDFDOM_DLLAPI std::string values2str(urdf::Vector3 vec); URDFDOM_DLLAPI std::string values2str(urdf::Rotation rot); URDFDOM_DLLAPI std::string values2str(urdf::Color c); -URDFDOM_DLLAPI std::string values2str(double d); } namespace urdf{ - URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string); URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string &path); URDFDOM_DLLAPI TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model); diff --git a/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp index 07cbeae1..20a38279 100644 --- a/urdf_parser/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -34,13 +34,16 @@ /* Author: John Hsu */ +#include +#include +#include +#include + #include #include #include -#include -#include -#include -#include + +#include "xml_helpers.h" namespace urdf{ @@ -639,32 +642,29 @@ bool exportPose(Pose &pose, TiXmlElement* xml); bool exportJointDynamics(JointDynamics &jd, TiXmlElement* xml) { - TiXmlElement *dynamics_xml = new TiXmlElement("dynamics"); - dynamics_xml->SetAttribute("damping", urdf_export_helpers::values2str(jd.damping) ); - dynamics_xml->SetAttribute("friction", urdf_export_helpers::values2str(jd.friction) ); - xml->LinkEndChild(dynamics_xml); + auto * dynamics_xml = xmlAppendChild(xml, "dynamics"); + dynamics_xml->SetAttribute("damping", jd.damping ); + dynamics_xml->SetAttribute("friction", jd.friction ); return true; } bool exportJointLimits(JointLimits &jl, TiXmlElement* xml) { - TiXmlElement *limit_xml = new TiXmlElement("limit"); - limit_xml->SetAttribute("effort", urdf_export_helpers::values2str(jl.effort) ); - limit_xml->SetAttribute("velocity", urdf_export_helpers::values2str(jl.velocity) ); - limit_xml->SetAttribute("lower", urdf_export_helpers::values2str(jl.lower) ); - limit_xml->SetAttribute("upper", urdf_export_helpers::values2str(jl.upper) ); - xml->LinkEndChild(limit_xml); + auto * limit_xml = xmlAppendChild(xml, "limit"); + limit_xml->SetAttribute("effort", jl.effort); + limit_xml->SetAttribute("velocity", jl.velocity ); + limit_xml->SetAttribute("lower", jl.lower ); + limit_xml->SetAttribute("upper", jl.upper ); return true; } bool exportJointSafety(JointSafety &js, TiXmlElement* xml) { - TiXmlElement *safety_xml = new TiXmlElement("safety_controller"); - safety_xml->SetAttribute("k_position", urdf_export_helpers::values2str(js.k_position) ); - safety_xml->SetAttribute("k_velocity", urdf_export_helpers::values2str(js.k_velocity) ); - safety_xml->SetAttribute("soft_lower_limit", urdf_export_helpers::values2str(js.soft_lower_limit) ); - safety_xml->SetAttribute("soft_upper_limit", urdf_export_helpers::values2str(js.soft_upper_limit) ); - xml->LinkEndChild(safety_xml); + auto * safety_xml = xmlAppendChild(xml, "safety_controller"); + safety_xml->SetAttribute("k_position", js.k_position ); + safety_xml->SetAttribute("k_velocity", js.k_velocity ); + safety_xml->SetAttribute("soft_lower_limit", js.soft_lower_limit ); + safety_xml->SetAttribute("soft_upper_limit", js.soft_upper_limit ); return true; } @@ -672,13 +672,12 @@ bool exportJointCalibration(JointCalibration &jc, TiXmlElement* xml) { if (jc.falling || jc.rising) { - TiXmlElement *calibration_xml = new TiXmlElement("calibration"); + auto * calibration_xml = xmlAppendChild(xml, "calibration"); if (jc.falling) - calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling) ); + calibration_xml->SetAttribute("falling", *jc.falling ); if (jc.rising) - calibration_xml->SetAttribute("rising", urdf_export_helpers::values2str(*jc.rising) ); + calibration_xml->SetAttribute("rising", *jc.rising ); //calibration_xml->SetAttribute("reference_position", urdf_export_helpers::values2str(jc.reference_position) ); - xml->LinkEndChild(calibration_xml); } return true; } @@ -687,19 +686,20 @@ bool exportJointMimic(JointMimic &jm, TiXmlElement* xml) { if (!jm.joint_name.empty()) { - TiXmlElement *mimic_xml = new TiXmlElement("mimic"); - mimic_xml->SetAttribute("offset", urdf_export_helpers::values2str(jm.offset) ); - mimic_xml->SetAttribute("multiplier", urdf_export_helpers::values2str(jm.multiplier) ); - mimic_xml->SetAttribute("joint", jm.joint_name ); - xml->LinkEndChild(mimic_xml); + auto * mimic_xml = xmlAppendChild(xml, "mimic"); + + mimic_xml->SetAttribute("offset", jm.offset ); + mimic_xml->SetAttribute("multiplier", jm.multiplier ); + mimic_xml->SetAttribute("joint", jm.joint_name.c_str() ); } return true; } bool exportJoint(Joint &joint, TiXmlElement* xml) { - TiXmlElement * joint_xml = new TiXmlElement("joint"); - joint_xml->SetAttribute("name", joint.name); + auto * joint_xml = xmlAppendChild(xml, "joint"); + + joint_xml->SetAttribute("name", joint.name.c_str()); if (joint.type == urdf::Joint::PLANAR) joint_xml->SetAttribute("type", "planar"); else if (joint.type == urdf::Joint::FLOATING) @@ -719,19 +719,16 @@ bool exportJoint(Joint &joint, TiXmlElement* xml) exportPose(joint.parent_to_joint_origin_transform, joint_xml); // axis - TiXmlElement * axis_xml = new TiXmlElement("axis"); - axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(joint.axis)); - joint_xml->LinkEndChild(axis_xml); + auto * axis_xml = xmlAppendChild(joint_xml, "axis"); + axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(joint.axis).c_str()); - // parent - TiXmlElement * parent_xml = new TiXmlElement("parent"); - parent_xml->SetAttribute("link", joint.parent_link_name); - joint_xml->LinkEndChild(parent_xml); + // parent + auto * parent_xml = xmlAppendChild(joint_xml, "parent"); + parent_xml->SetAttribute("link", joint.parent_link_name.c_str()); // child - TiXmlElement * child_xml = new TiXmlElement("child"); - child_xml->SetAttribute("link", joint.child_link_name); - joint_xml->LinkEndChild(child_xml); + auto * child_xml = xmlAppendChild(joint_xml, "child"); + child_xml->SetAttribute("link", joint.child_link_name.c_str()); if (joint.dynamics) exportJointDynamics(*(joint.dynamics), joint_xml); @@ -744,7 +741,6 @@ bool exportJoint(Joint &joint, TiXmlElement* xml) if (joint.mimic) exportJointMimic(*(joint.mimic), joint_xml); - xml->LinkEndChild(joint_xml); return true; } diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index 0e8b8c30..79afa8a2 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -34,16 +34,18 @@ /* Author: Wim Meeussen */ - -#include +#include +#include #include +#include + +#include #include #include #include #include -#include -#include -#include + +#include "xml_helpers.h" namespace urdf{ @@ -254,7 +256,7 @@ GeometrySharedPtr parseGeometry(TiXmlElement *g) return geom; } - std::string type_name = shape->ValueStr(); + std::string type_name = shape->Name(); if (type_name == "sphere") { Sphere *s = new Sphere(); @@ -532,63 +534,56 @@ bool exportPose(Pose &pose, TiXmlElement* xml); bool exportMaterial(Material &material, TiXmlElement *xml) { - TiXmlElement *material_xml = new TiXmlElement("material"); - material_xml->SetAttribute("name", material.name); + auto material_xml = xmlAppendChild(xml, "material"); + material_xml->SetAttribute("name", material.name.c_str()); - TiXmlElement* texture = new TiXmlElement("texture"); + auto texture = xmlAppendChild(material_xml,"texture"); if (!material.texture_filename.empty()) - texture->SetAttribute("filename", material.texture_filename); - material_xml->LinkEndChild(texture); + texture->SetAttribute("filename", material.texture_filename.c_str()); - TiXmlElement* color = new TiXmlElement("color"); - color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color)); - material_xml->LinkEndChild(color); - xml->LinkEndChild(material_xml); + auto color = xmlAppendChild(material_xml, "color"); + color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color).c_str()); return true; } bool exportSphere(Sphere &s, TiXmlElement *xml) { // e.g. add - TiXmlElement *sphere_xml = new TiXmlElement("sphere"); - sphere_xml->SetAttribute("radius", urdf_export_helpers::values2str(s.radius)); - xml->LinkEndChild(sphere_xml); + auto sphere_xml = xmlAppendChild(xml, "sphere"); + sphere_xml->SetAttribute("radius", s.radius); return true; } bool exportBox(Box &b, TiXmlElement *xml) { // e.g. add - TiXmlElement *box_xml = new TiXmlElement("box"); - box_xml->SetAttribute("size", urdf_export_helpers::values2str(b.dim)); - xml->LinkEndChild(box_xml); + auto box_xml = xmlAppendChild(xml, "box"); + box_xml->SetAttribute("size", urdf_export_helpers::values2str(b.dim).c_str()); return true; } bool exportCylinder(Cylinder &y, TiXmlElement *xml) { // e.g. add - TiXmlElement *cylinder_xml = new TiXmlElement("cylinder"); - cylinder_xml->SetAttribute("radius", urdf_export_helpers::values2str(y.radius)); - cylinder_xml->SetAttribute("length", urdf_export_helpers::values2str(y.length)); - xml->LinkEndChild(cylinder_xml); + auto cylinder_xml = xmlAppendChild(xml, "cylinder"); + cylinder_xml->SetAttribute("radius", y.radius); + cylinder_xml->SetAttribute("length", y.length); return true; } bool exportMesh(Mesh &m, TiXmlElement *xml) { // e.g. add - TiXmlElement *mesh_xml = new TiXmlElement("mesh"); + auto mesh_xml = xmlAppendChild(xml, "mesh"); if (!m.filename.empty()) - mesh_xml->SetAttribute("filename", m.filename); - mesh_xml->SetAttribute("scale", urdf_export_helpers::values2str(m.scale)); - xml->LinkEndChild(mesh_xml); + mesh_xml->SetAttribute("filename", m.filename.c_str()); + mesh_xml->SetAttribute("scale", urdf_export_helpers::values2str(m.scale).c_str()); return true; } bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml) { - TiXmlElement *geometry_xml = new TiXmlElement("geometry"); + TiXmlElement *geometry_xml = xmlAppendChild(xml,"geometry"); if (urdf::dynamic_pointer_cast(geom)) { exportSphere((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); @@ -614,7 +609,6 @@ bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml) exportSphere((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); } - xml->LinkEndChild(geometry_xml); return true; } @@ -625,24 +619,20 @@ bool exportInertial(Inertial &i, TiXmlElement *xml) // // // - TiXmlElement *inertial_xml = new TiXmlElement("inertial"); + auto inertial_xml = xmlAppendChild(xml, "inertial"); - TiXmlElement *mass_xml = new TiXmlElement("mass"); - mass_xml->SetAttribute("value", urdf_export_helpers::values2str(i.mass)); - inertial_xml->LinkEndChild(mass_xml); + auto mass_xml = xmlAppendChild(inertial_xml, "mass"); + mass_xml->SetAttribute("value", i.mass); exportPose(i.origin, inertial_xml); - TiXmlElement *inertia_xml = new TiXmlElement("inertia"); - inertia_xml->SetAttribute("ixx", urdf_export_helpers::values2str(i.ixx)); - inertia_xml->SetAttribute("ixy", urdf_export_helpers::values2str(i.ixy)); - inertia_xml->SetAttribute("ixz", urdf_export_helpers::values2str(i.ixz)); - inertia_xml->SetAttribute("iyy", urdf_export_helpers::values2str(i.iyy)); - inertia_xml->SetAttribute("iyz", urdf_export_helpers::values2str(i.iyz)); - inertia_xml->SetAttribute("izz", urdf_export_helpers::values2str(i.izz)); - inertial_xml->LinkEndChild(inertia_xml); - - xml->LinkEndChild(inertial_xml); + auto inertia_xml = xmlAppendChild(inertial_xml, "inertia"); + inertia_xml->SetAttribute("ixx", i.ixx); + inertia_xml->SetAttribute("ixy", i.ixy); + inertia_xml->SetAttribute("ixz", i.ixz); + inertia_xml->SetAttribute("iyy", i.iyy); + inertia_xml->SetAttribute("iyz", i.iyz); + inertia_xml->SetAttribute("izz", i.izz); return true; } @@ -656,7 +646,7 @@ bool exportVisual(Visual &vis, TiXmlElement *xml) // // // - TiXmlElement * visual_xml = new TiXmlElement("visual"); + auto visual_xml = xmlAppendChild(xml, "visual"); exportPose(vis.origin, visual_xml); @@ -665,8 +655,6 @@ bool exportVisual(Visual &vis, TiXmlElement *xml) if (vis.material) exportMaterial(*vis.material, visual_xml); - xml->LinkEndChild(visual_xml); - return true; } @@ -679,21 +667,19 @@ bool exportCollision(Collision &col, TiXmlElement* xml) // // // - TiXmlElement * collision_xml = new TiXmlElement("collision"); + auto collision_xml = xmlAppendChild(xml, "collision"); exportPose(col.origin, collision_xml); exportGeometry(col.geometry, collision_xml); - xml->LinkEndChild(collision_xml); - return true; } bool exportLink(Link &link, TiXmlElement* xml) { - TiXmlElement * link_xml = new TiXmlElement("link"); - link_xml->SetAttribute("name", link.name); + auto link_xml = xmlAppendChild(xml, "link"); + link_xml->SetAttribute("name", link.name.c_str()); if (link.inertial) exportInertial(*link.inertial, link_xml); @@ -702,8 +688,6 @@ bool exportLink(Link &link, TiXmlElement* xml) for (std::size_t i = 0 ; i < link.collision_array.size() ; ++i) exportCollision(*link.collision_array[i], link_xml); - xml->LinkEndChild(link_xml); - return true; } diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index a905f89e..617f2a2e 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -34,10 +34,13 @@ /* Author: Wim Meeussen */ -#include -#include "urdf_parser/urdf_parser.h" #include + #include +#include + +#include "urdf_parser/urdf_parser.h" +#include "xml_helpers.h" namespace urdf{ @@ -68,7 +71,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) xml_doc.Parse(xml_string.c_str()); if (xml_doc.Error()) { - CONSOLE_BRIDGE_logError(xml_doc.ErrorDesc()); + CONSOLE_BRIDGE_logError(xml_doc.ErrorStr()); xml_doc.ClearError(); model.reset(); return model; @@ -248,11 +251,8 @@ bool exportJoint(Joint &joint, TiXmlElement *config); TiXmlDocument* exportURDF(const ModelInterface &model) { TiXmlDocument *doc = new TiXmlDocument(); - - TiXmlElement *robot = new TiXmlElement("robot"); - robot->SetAttribute("name", model.name_); - doc->LinkEndChild(robot); - + auto robot = xmlAppendChild(doc, "robot"); + robot->SetAttribute("name", model.name_.c_str()); for (std::map::const_iterator m=model.materials_.begin(); m!=model.materials_.end(); m++) { diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp index 56bedd48..278ff713 100644 --- a/urdf_parser/src/pose.cpp +++ b/urdf_parser/src/pose.cpp @@ -40,9 +40,11 @@ #include #include #include -#include +#include #include +#include "xml_helpers.h" + namespace urdf_export_helpers { std::string values2str(unsigned int count, const double *values, double (*conv)(double)) @@ -79,10 +81,6 @@ std::string values2str(urdf::Color c) rgba[3] = c.a; return values2str(4, rgba); } -std::string values2str(double d) -{ - return values2str(1, &d); -} } namespace urdf{ @@ -121,12 +119,11 @@ bool parsePose(Pose &pose, TiXmlElement* xml) bool exportPose(Pose &pose, TiXmlElement* xml) { - TiXmlElement *origin = new TiXmlElement("origin"); + auto * origin = xmlAppendChild(xml,"origin"); std::string pose_xyz_str = urdf_export_helpers::values2str(pose.position); std::string pose_rpy_str = urdf_export_helpers::values2str(pose.rotation); - origin->SetAttribute("xyz", pose_xyz_str); - origin->SetAttribute("rpy", pose_rpy_str); - xml->LinkEndChild(origin); + origin->SetAttribute("xyz", pose_xyz_str.c_str()); + origin->SetAttribute("rpy", pose_rpy_str.c_str()); return true; } diff --git a/urdf_parser/src/twist.cpp b/urdf_parser/src/twist.cpp index 694fc914..7cfb022c 100644 --- a/urdf_parser/src/twist.cpp +++ b/urdf_parser/src/twist.cpp @@ -39,7 +39,8 @@ #include #include #include -#include +#include +#include #include namespace urdf{ diff --git a/urdf_parser/src/urdf_model_state.cpp b/urdf_parser/src/urdf_model_state.cpp index 447d4ff6..7c9fcd0f 100644 --- a/urdf_parser/src/urdf_model_state.cpp +++ b/urdf_parser/src/urdf_model_state.cpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include diff --git a/urdf_parser/src/urdf_sensor.cpp b/urdf_parser/src/urdf_sensor.cpp index e13f780b..229965eb 100644 --- a/urdf_parser/src/urdf_sensor.cpp +++ b/urdf_parser/src/urdf_sensor.cpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include diff --git a/urdf_parser/src/world.cpp b/urdf_parser/src/world.cpp index ce09c1c3..3a025887 100644 --- a/urdf_parser/src/world.cpp +++ b/urdf_parser/src/world.cpp @@ -34,15 +34,17 @@ /* Author: Wim Meeussen */ - -#include +#include +#include #include #include +#include + +#include #include #include -#include -#include -#include + +#include "xml_helpers.h" namespace urdf{ @@ -56,14 +58,12 @@ bool parseWorld(World &/*world*/, TiXmlElement* /*config*/) bool exportWorld(World &world, TiXmlElement* xml) { - TiXmlElement * world_xml = new TiXmlElement("world"); - world_xml->SetAttribute("name", world.name); + auto world_xml = xmlAppendChild(xml, "world"); + world_xml->SetAttribute("name", world.name.c_str()); // to be implemented // exportModels(*world.models, world_xml); - xml->LinkEndChild(world_xml); - return true; } diff --git a/urdf_parser/src/xml_helpers.cpp b/urdf_parser/src/xml_helpers.cpp new file mode 100644 index 00000000..1006e06f --- /dev/null +++ b/urdf_parser/src/xml_helpers.cpp @@ -0,0 +1,41 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2020 Rover Robotics c/o Dan Rose +* 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. +*********************************************************************/ + +#include "xml_helpers.h" + +tinyxml2::XMLElement* urdf::xmlAppendChild(tinyxml2::XMLNode * parent, const char * new_tag) { + auto e = parent->GetDocument()->NewElement(new_tag); + parent->LinkEndChild(e); + return e; +} diff --git a/urdf_parser/src/xml_helpers.h b/urdf_parser/src/xml_helpers.h new file mode 100644 index 00000000..d095d40c --- /dev/null +++ b/urdf_parser/src/xml_helpers.h @@ -0,0 +1,43 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2020 Rover Robotics c/o Dan Rose +* 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. +*********************************************************************/ + +#ifndef ROS2_MASTER_XML_HELPERS_HPP +#define ROS2_MASTER_XML_HELPERS_HPP + +#include +namespace urdf +{ + tinyxml2::XMLElement * xmlAppendChild(tinyxml2::XMLNode * parent, const char * new_tag); +} +#endif //ROS2_MASTER_XML_HELPERS_HPP