From 9c68be5fa7be449b500426fa663a86fdd008bbcf Mon Sep 17 00:00:00 2001 From: Dmitry Rozhkov Date: Thu, 26 Jan 2017 12:11:51 +0200 Subject: [PATCH 1/8] Switch from TinyXML to TinyXML2 The library TinyXML is considered to be unmaintained and since all future development is focused on TinyXML2 this patch updates urdfdom to use TinyXML2. Signed-off-by: Dmitry Rozhkov --- CMakeLists.txt | 17 +- cmake/FindTinyXML.cmake | 74 ------- cmake/FindTinyXML2.cmake | 74 +++++++ urdf_parser/include/urdf_parser/urdf_parser.h | 18 +- urdf_parser/src/joint.cpp | 134 +++++++------ urdf_parser/src/link.cpp | 188 ++++++++++-------- urdf_parser/src/model.cpp | 42 ++-- urdf_parser/src/pose.cpp | 17 +- urdf_parser/src/twist.cpp | 6 +- urdf_parser/src/urdf_model_state.cpp | 7 +- urdf_parser/src/urdf_sensor.cpp | 23 +-- urdf_parser/src/world.cpp | 14 +- 12 files changed, 327 insertions(+), 287 deletions(-) delete mode 100644 cmake/FindTinyXML.cmake create mode 100644 cmake/FindTinyXML2.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index afc5f9f1..93f01cdd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,15 +45,14 @@ endif (MSVC OR MSVC90 OR MSVC10) set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") -find_package(tinyxml_vendor QUIET) -find_package(TinyXML) -# bionic has not cmake module, workaround -if (NOT TinyXML_FOUND AND UNIX) - include(FindPkgConfig) - pkg_check_modules (TinyXML tinyxml) -else() - # Make it fail in platforms without pkgconfig - find_package(TinyXML REQUIRED) # bionic has not cmake module +find_package(TinyXML2 REQUIRED) +include_directories(SYSTEM ${TinyXML2_INCLUDE_DIRS}) +link_libraries(${TinyXML2_LIBRARIES}) + +find_package(urdfdom_headers 1.0 REQUIRED) +include_directories(SYSTEM ${urdfdom_headers_INCLUDE_DIRS}) +if (NOT MSVC) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") endif() find_package(urdfdom_headers 1.0 REQUIRED) find_package(console_bridge_vendor QUIET) # Provides console_bridge 0.4.0 on platforms without it. 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/FindTinyXML2.cmake b/cmake/FindTinyXML2.cmake new file mode 100644 index 00000000..87ff2fbc --- /dev/null +++ b/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/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index a293379e..eaf4c871 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -40,8 +40,8 @@ #include #include #include - -#include +#include +#include #include #include #include @@ -143,13 +143,13 @@ 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); - URDFDOM_DLLAPI TiXmlDocument* exportURDF(const ModelInterface &model); - URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*); - URDFDOM_DLLAPI bool parseCamera(Camera&, TiXmlElement*); - URDFDOM_DLLAPI bool parseRay(Ray&, TiXmlElement*); - URDFDOM_DLLAPI bool parseSensor(Sensor&, TiXmlElement*); - URDFDOM_DLLAPI bool parseModelState(ModelState&, TiXmlElement*); + URDFDOM_DLLAPI tinyxml2::XMLElement* exportURDF(ModelInterfaceSharedPtr &model); + URDFDOM_DLLAPI tinyxml2::XMLElement* exportURDF(const ModelInterface &model); + URDFDOM_DLLAPI bool parsePose(Pose&, tinyxml2::XMLElement*); + URDFDOM_DLLAPI bool parseCamera(Camera&, tinyxml2::XMLElement*); + URDFDOM_DLLAPI bool parseRay(Ray&, tinyxml2::XMLElement*); + URDFDOM_DLLAPI bool parseSensor(Sensor&, tinyxml2::XMLElement*); + URDFDOM_DLLAPI bool parseModelState(ModelState&, tinyxml2::XMLElement*); } #endif diff --git a/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp index d12dcb7c..78aee4f9 100644 --- a/urdf_parser/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -40,14 +40,16 @@ #include #include #include -#include +#include #include +using namespace tinyxml2; + namespace urdf{ -bool parsePose(Pose &pose, TiXmlElement* xml); +bool parsePose(Pose &pose, XMLElement* xml); -bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) +bool parseJointDynamics(JointDynamics &jd, XMLElement* config) { jd.clear(); @@ -94,7 +96,7 @@ bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) } } -bool parseJointLimits(JointLimits &jl, TiXmlElement* config) +bool parseJointLimits(JointLimits &jl, XMLElement* config) { jl.clear(); @@ -165,7 +167,7 @@ bool parseJointLimits(JointLimits &jl, TiXmlElement* config) return true; } -bool parseJointSafety(JointSafety &js, TiXmlElement* config) +bool parseJointSafety(JointSafety &js, XMLElement* config) { js.clear(); @@ -239,7 +241,7 @@ bool parseJointSafety(JointSafety &js, TiXmlElement* config) return true; } -bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config) +bool parseJointCalibration(JointCalibration &jc, XMLElement* config) { jc.clear(); @@ -280,7 +282,7 @@ bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config) return true; } -bool parseJointMimic(JointMimic &jm, TiXmlElement* config) +bool parseJointMimic(JointMimic &jm, XMLElement* config) { jm.clear(); @@ -334,7 +336,7 @@ bool parseJointMimic(JointMimic &jm, TiXmlElement* config) return true; } -bool parseJoint(Joint &joint, TiXmlElement* config) +bool parseJoint(Joint &joint, XMLElement* config) { joint.clear(); @@ -348,7 +350,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) joint.name = name; // Get transform from Parent Link to Joint Frame - TiXmlElement *origin_xml = config->FirstChildElement("origin"); + XMLElement *origin_xml = config->FirstChildElement("origin"); if (!origin_xml) { CONSOLE_BRIDGE_logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str()); @@ -365,7 +367,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get Parent Link - TiXmlElement *parent_xml = config->FirstChildElement("parent"); + XMLElement *parent_xml = config->FirstChildElement("parent"); if (parent_xml) { const char *pname = parent_xml->Attribute("link"); @@ -380,7 +382,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get Child Link - TiXmlElement *child_xml = config->FirstChildElement("child"); + XMLElement *child_xml = config->FirstChildElement("child"); if (child_xml) { const char *pname = child_xml->Attribute("link"); @@ -425,7 +427,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED) { // axis - TiXmlElement *axis_xml = config->FirstChildElement("axis"); + XMLElement *axis_xml = config->FirstChildElement("axis"); if (!axis_xml){ CONSOLE_BRIDGE_logDebug("urdfdom: no axis element for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str()); joint.axis = Vector3(1.0, 0.0, 0.0); @@ -445,7 +447,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get limit - TiXmlElement *limit_xml = config->FirstChildElement("limit"); + XMLElement *limit_xml = config->FirstChildElement("limit"); if (limit_xml) { joint.limits.reset(new JointLimits()); @@ -468,7 +470,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get safety - TiXmlElement *safety_xml = config->FirstChildElement("safety_controller"); + XMLElement *safety_xml = config->FirstChildElement("safety_controller"); if (safety_xml) { joint.safety.reset(new JointSafety()); @@ -481,7 +483,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get calibration - TiXmlElement *calibration_xml = config->FirstChildElement("calibration"); + XMLElement *calibration_xml = config->FirstChildElement("calibration"); if (calibration_xml) { joint.calibration.reset(new JointCalibration()); @@ -494,7 +496,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get Joint Mimic - TiXmlElement *mimic_xml = config->FirstChildElement("mimic"); + XMLElement *mimic_xml = config->FirstChildElement("mimic"); if (mimic_xml) { joint.mimic.reset(new JointMimic()); @@ -507,7 +509,7 @@ bool parseJoint(Joint &joint, TiXmlElement* config) } // Get Dynamics - TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); + XMLElement *prop_xml = config->FirstChildElement("dynamics"); if (prop_xml) { joint.dynamics.reset(new JointDynamics()); @@ -524,71 +526,83 @@ bool parseJoint(Joint &joint, TiXmlElement* config) /* exports */ -bool exportPose(Pose &pose, TiXmlElement* xml); +bool exportPose(Pose &pose, XMLElement* xml); -bool exportJointDynamics(JointDynamics &jd, TiXmlElement* xml) +bool exportJointDynamics(JointDynamics &jd, XMLElement* 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); + XMLDocument *doc = xml->GetDocument(); + + XMLElement *dynamics_xml = doc->NewElement("dynamics"); + dynamics_xml->SetAttribute("damping", urdf_export_helpers::values2str(jd.damping).c_str() ); + dynamics_xml->SetAttribute("friction", urdf_export_helpers::values2str(jd.friction).c_str() ); + xml->InsertEndChild(dynamics_xml); return true; } -bool exportJointLimits(JointLimits &jl, TiXmlElement* xml) +bool exportJointLimits(JointLimits &jl, XMLElement* 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); + XMLDocument *doc = xml->GetDocument(); + + XMLElement *limit_xml = doc->NewElement("limit"); + limit_xml->SetAttribute("effort", urdf_export_helpers::values2str(jl.effort).c_str() ); + limit_xml->SetAttribute("velocity", urdf_export_helpers::values2str(jl.velocity).c_str() ); + limit_xml->SetAttribute("lower", urdf_export_helpers::values2str(jl.lower).c_str() ); + limit_xml->SetAttribute("upper", urdf_export_helpers::values2str(jl.upper).c_str() ); + xml->InsertEndChild(limit_xml); return true; } -bool exportJointSafety(JointSafety &js, TiXmlElement* xml) +bool exportJointSafety(JointSafety &js, XMLElement* 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); + XMLDocument *doc = xml->GetDocument(); + + XMLElement *safety_xml = doc->NewElement("safety_controller"); + safety_xml->SetAttribute("k_position", urdf_export_helpers::values2str(js.k_position).c_str() ); + safety_xml->SetAttribute("k_velocity", urdf_export_helpers::values2str(js.k_velocity).c_str() ); + safety_xml->SetAttribute("soft_lower_limit", urdf_export_helpers::values2str(js.soft_lower_limit).c_str() ); + safety_xml->SetAttribute("soft_upper_limit", urdf_export_helpers::values2str(js.soft_upper_limit).c_str() ); + xml->InsertEndChild(safety_xml); return true; } -bool exportJointCalibration(JointCalibration &jc, TiXmlElement* xml) +bool exportJointCalibration(JointCalibration &jc, XMLElement* xml) { + XMLDocument *doc = xml->GetDocument(); + if (jc.falling || jc.rising) { - TiXmlElement *calibration_xml = new TiXmlElement("calibration"); + XMLElement *calibration_xml = doc->NewElement("calibration"); if (jc.falling) - calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling) ); + calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling).c_str() ); if (jc.rising) - calibration_xml->SetAttribute("rising", urdf_export_helpers::values2str(*jc.rising) ); + calibration_xml->SetAttribute("rising", urdf_export_helpers::values2str(*jc.rising).c_str() ); //calibration_xml->SetAttribute("reference_position", urdf_export_helpers::values2str(jc.reference_position) ); - xml->LinkEndChild(calibration_xml); + xml->InsertEndChild(calibration_xml); } return true; } -bool exportJointMimic(JointMimic &jm, TiXmlElement* xml) +bool exportJointMimic(JointMimic &jm, XMLElement* xml) { + XMLDocument *doc = xml->GetDocument(); + 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); + XMLElement *mimic_xml = doc->NewElement("mimic"); + mimic_xml->SetAttribute("offset", urdf_export_helpers::values2str(jm.offset).c_str() ); + mimic_xml->SetAttribute("multiplier", urdf_export_helpers::values2str(jm.multiplier).c_str() ); + mimic_xml->SetAttribute("joint", jm.joint_name.c_str() ); + xml->InsertEndChild(mimic_xml); } return true; } -bool exportJoint(Joint &joint, TiXmlElement* xml) +bool exportJoint(Joint &joint, XMLElement* xml) { - TiXmlElement * joint_xml = new TiXmlElement("joint"); - joint_xml->SetAttribute("name", joint.name); + XMLDocument *doc = xml->GetDocument(); + + XMLElement * joint_xml = doc->NewElement("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) @@ -608,19 +622,19 @@ 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); + XMLElement * axis_xml = doc->NewElement("axis"); + axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(joint.axis).c_str()); + joint_xml->InsertEndChild(axis_xml); // parent - TiXmlElement * parent_xml = new TiXmlElement("parent"); - parent_xml->SetAttribute("link", joint.parent_link_name); + XMLElement * parent_xml = doc-> NewElement("parent"); + parent_xml->SetAttribute("link", joint.parent_link_name.c_str()); joint_xml->LinkEndChild(parent_xml); // child - TiXmlElement * child_xml = new TiXmlElement("child"); - child_xml->SetAttribute("link", joint.child_link_name); - joint_xml->LinkEndChild(child_xml); + XMLElement * child_xml = doc->NewElement("child"); + child_xml->SetAttribute("link", joint.child_link_name.c_str()); + joint_xml->InsertEndChild(child_xml); if (joint.dynamics) exportJointDynamics(*(joint.dynamics), joint_xml); @@ -633,7 +647,7 @@ bool exportJoint(Joint &joint, TiXmlElement* xml) if (joint.mimic) exportJointMimic(*(joint.mimic), joint_xml); - xml->LinkEndChild(joint_xml); + xml->InsertEndChild(joint_xml); return true; } diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index a09ce381..61f00ac9 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -45,14 +45,16 @@ #include #include #include -#include +#include #include +using namespace tinyxml2; + namespace urdf{ -bool parsePose(Pose &pose, TiXmlElement* xml); +bool parsePose(Pose &pose, XMLElement* xml); -bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok) +bool parseMaterial(Material &material, XMLElement *config, bool only_name_is_ok) { bool has_rgb = false; bool has_filename = false; @@ -68,7 +70,7 @@ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_o material.name = config->Attribute("name"); // texture - TiXmlElement *t = config->FirstChildElement("texture"); + XMLElement *t = config->FirstChildElement("texture"); if (t) { if (t->Attribute("filename")) @@ -79,7 +81,7 @@ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_o } // color - TiXmlElement *c = config->FirstChildElement("color"); + XMLElement *c = config->FirstChildElement("color"); if (c) { if (c->Attribute("rgba")) { @@ -107,7 +109,7 @@ bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_o } -bool parseSphere(Sphere &s, TiXmlElement *c) +bool parseSphere(Sphere &s, XMLElement *c) { s.clear(); @@ -130,7 +132,7 @@ bool parseSphere(Sphere &s, TiXmlElement *c) return true; } -bool parseBox(Box &b, TiXmlElement *c) +bool parseBox(Box &b, XMLElement *c) { b.clear(); @@ -153,7 +155,7 @@ bool parseBox(Box &b, TiXmlElement *c) return true; } -bool parseCylinder(Cylinder &y, TiXmlElement *c) +bool parseCylinder(Cylinder &y, XMLElement *c) { y.clear(); @@ -187,7 +189,7 @@ bool parseCylinder(Cylinder &y, TiXmlElement *c) } -bool parseMesh(Mesh &m, TiXmlElement *c) +bool parseMesh(Mesh &m, XMLElement *c) { m.clear(); @@ -216,19 +218,19 @@ bool parseMesh(Mesh &m, TiXmlElement *c) return true; } -GeometrySharedPtr parseGeometry(TiXmlElement *g) +GeometrySharedPtr parseGeometry(XMLElement *g) { GeometrySharedPtr geom; if (!g) return geom; - TiXmlElement *shape = g->FirstChildElement(); + XMLElement *shape = g->FirstChildElement(); if (!shape) { CONSOLE_BRIDGE_logError("Geometry tag contains no child element."); return geom; } - std::string type_name = shape->ValueStr(); + std::string type_name(shape->Value()); if (type_name == "sphere") { Sphere *s = new Sphere(); @@ -266,19 +268,19 @@ GeometrySharedPtr parseGeometry(TiXmlElement *g) return GeometrySharedPtr(); } -bool parseInertial(Inertial &i, TiXmlElement *config) +bool parseInertial(Inertial &i, XMLElement *config) { i.clear(); // Origin - TiXmlElement *o = config->FirstChildElement("origin"); + XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(i.origin, o)) return false; } - TiXmlElement *mass_xml = config->FirstChildElement("mass"); + XMLElement *mass_xml = config->FirstChildElement("mass"); if (!mass_xml) { CONSOLE_BRIDGE_logError("Inertial element must have a mass element"); @@ -300,7 +302,7 @@ bool parseInertial(Inertial &i, TiXmlElement *config) return false; } - TiXmlElement *inertia_xml = config->FirstChildElement("inertia"); + XMLElement *inertia_xml = config->FirstChildElement("inertia"); if (!inertia_xml) { CONSOLE_BRIDGE_logError("Inertial element must have inertia element"); @@ -346,19 +348,19 @@ bool parseInertial(Inertial &i, TiXmlElement *config) return true; } -bool parseVisual(Visual &vis, TiXmlElement *config) +bool parseVisual(Visual &vis, XMLElement *config) { vis.clear(); // Origin - TiXmlElement *o = config->FirstChildElement("origin"); + XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(vis.origin, o)) return false; } // Geometry - TiXmlElement *geom = config->FirstChildElement("geometry"); + XMLElement *geom = config->FirstChildElement("geometry"); vis.geometry = parseGeometry(geom); if (!vis.geometry) return false; @@ -368,7 +370,7 @@ bool parseVisual(Visual &vis, TiXmlElement *config) vis.name = name_char; // Material - TiXmlElement *mat = config->FirstChildElement("material"); + XMLElement *mat = config->FirstChildElement("material"); if (mat) { // get material name if (!mat->Attribute("name")) { @@ -388,19 +390,19 @@ bool parseVisual(Visual &vis, TiXmlElement *config) return true; } -bool parseCollision(Collision &col, TiXmlElement* config) +bool parseCollision(Collision &col, XMLElement* config) { col.clear(); // Origin - TiXmlElement *o = config->FirstChildElement("origin"); + XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(col.origin, o)) return false; } // Geometry - TiXmlElement *geom = config->FirstChildElement("geometry"); + XMLElement *geom = config->FirstChildElement("geometry"); col.geometry = parseGeometry(geom); if (!col.geometry) return false; @@ -412,7 +414,7 @@ bool parseCollision(Collision &col, TiXmlElement* config) return true; } -bool parseLink(Link &link, TiXmlElement* config) +bool parseLink(Link &link, XMLElement* config) { link.clear(); @@ -426,7 +428,7 @@ bool parseLink(Link &link, TiXmlElement* config) link.name = std::string(name_char); // Inertial (optional) - TiXmlElement *i = config->FirstChildElement("inertial"); + XMLElement *i = config->FirstChildElement("inertial"); if (i) { link.inertial.reset(new Inertial()); @@ -438,7 +440,7 @@ bool parseLink(Link &link, TiXmlElement* config) } // Multiple Visuals (optional) - for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) + for (XMLElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) { VisualSharedPtr vis; @@ -461,7 +463,7 @@ bool parseLink(Link &link, TiXmlElement* config) link.visual = link.visual_array[0]; // Multiple Collisions (optional) - for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) + for (XMLElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) { CollisionSharedPtr col; col.reset(new Collision()); @@ -486,67 +488,79 @@ bool parseLink(Link &link, TiXmlElement* config) } /* exports */ -bool exportPose(Pose &pose, TiXmlElement* xml); +bool exportPose(Pose &pose, XMLElement* xml); -bool exportMaterial(Material &material, TiXmlElement *xml) +bool exportMaterial(Material &material, XMLElement *xml) { - TiXmlElement *material_xml = new TiXmlElement("material"); - material_xml->SetAttribute("name", material.name); + XMLDocument *doc = xml->GetDocument(); + + XMLElement *material_xml = doc->NewElement("material"); + material_xml->SetAttribute("name", material.name.c_str()); - TiXmlElement* texture = new TiXmlElement("texture"); + XMLElement* texture = doc->NewElement("texture"); if (!material.texture_filename.empty()) - texture->SetAttribute("filename", material.texture_filename); - material_xml->LinkEndChild(texture); + texture->SetAttribute("filename", material.texture_filename.c_str()); + material_xml->InsertEndChild(texture); - TiXmlElement* color = new TiXmlElement("color"); - color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color)); - material_xml->LinkEndChild(color); - xml->LinkEndChild(material_xml); + XMLElement* color = doc->NewElement("color"); + color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color).c_str()); + material_xml->InsertEndChild(color); + xml->InsertEndChild(material_xml); return true; } -bool exportSphere(Sphere &s, TiXmlElement *xml) +bool exportSphere(Sphere &s, XMLElement *xml) { + XMLDocument *doc = xml->GetDocument(); + // e.g. add - TiXmlElement *sphere_xml = new TiXmlElement("sphere"); - sphere_xml->SetAttribute("radius", urdf_export_helpers::values2str(s.radius)); - xml->LinkEndChild(sphere_xml); + XMLElement *sphere_xml = doc->NewElement("sphere"); + sphere_xml->SetAttribute("radius", urdf_export_helpers::values2str(s.radius).c_str()); + xml->InsertEndChild(sphere_xml); return true; } -bool exportBox(Box &b, TiXmlElement *xml) +bool exportBox(Box &b, XMLElement *xml) { + XMLDocument *doc = xml->GetDocument(); + // e.g. add - TiXmlElement *box_xml = new TiXmlElement("box"); - box_xml->SetAttribute("size", urdf_export_helpers::values2str(b.dim)); - xml->LinkEndChild(box_xml); + XMLElement *box_xml = doc->NewElement("box"); + box_xml->SetAttribute("size", urdf_export_helpers::values2str(b.dim).c_str()); + xml->InsertEndChild(box_xml); return true; } -bool exportCylinder(Cylinder &y, TiXmlElement *xml) +bool exportCylinder(Cylinder &y, XMLElement *xml) { + XMLDocument *doc = xml->GetDocument(); + // 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); + XMLElement *cylinder_xml = doc->NewElement("cylinder"); + cylinder_xml->SetAttribute("radius", urdf_export_helpers::values2str(y.radius).c_str()); + cylinder_xml->SetAttribute("length", urdf_export_helpers::values2str(y.length).c_str()); + xml->InsertEndChild(cylinder_xml); return true; } -bool exportMesh(Mesh &m, TiXmlElement *xml) +bool exportMesh(Mesh &m, XMLElement *xml) { + XMLDocument *doc = xml->GetDocument(); + // e.g. add - TiXmlElement *mesh_xml = new TiXmlElement("mesh"); + XMLElement *mesh_xml = doc->NewElement("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()); + xml->InsertEndChild(mesh_xml); return true; } -bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml) +bool exportGeometry(GeometrySharedPtr &geom, XMLElement *xml) { - TiXmlElement *geometry_xml = new TiXmlElement("geometry"); + XMLDocument *doc = xml->GetDocument(); + + XMLElement *geometry_xml = doc->NewElement("geometry"); if (urdf::dynamic_pointer_cast(geom)) { exportSphere((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); @@ -572,40 +586,42 @@ bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml) exportSphere((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); } - xml->LinkEndChild(geometry_xml); + xml->InsertEndChild(geometry_xml); return true; } -bool exportInertial(Inertial &i, TiXmlElement *xml) +bool exportInertial(Inertial &i, XMLElement *xml) { // adds // // // // - TiXmlElement *inertial_xml = new TiXmlElement("inertial"); + XMLDocument *doc = xml->GetDocument(); + + XMLElement *inertial_xml = doc->NewElement("inertial"); - TiXmlElement *mass_xml = new TiXmlElement("mass"); - mass_xml->SetAttribute("value", urdf_export_helpers::values2str(i.mass)); - inertial_xml->LinkEndChild(mass_xml); + XMLElement *mass_xml = doc->NewElement("mass"); + mass_xml->SetAttribute("value", urdf_export_helpers::values2str(i.mass).c_str()); + inertial_xml->InsertEndChild(mass_xml); 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); + XMLElement *inertia_xml = doc->NewElement("inertia"); + inertia_xml->SetAttribute("ixx", urdf_export_helpers::values2str(i.ixx).c_str()); + inertia_xml->SetAttribute("ixy", urdf_export_helpers::values2str(i.ixy).c_str()); + inertia_xml->SetAttribute("ixz", urdf_export_helpers::values2str(i.ixz).c_str()); + inertia_xml->SetAttribute("iyy", urdf_export_helpers::values2str(i.iyy).c_str()); + inertia_xml->SetAttribute("iyz", urdf_export_helpers::values2str(i.iyz).c_str()); + inertia_xml->SetAttribute("izz", urdf_export_helpers::values2str(i.izz).c_str()); + inertial_xml->InsertEndChild(inertia_xml); - xml->LinkEndChild(inertial_xml); + xml->InsertEndChild(inertial_xml); return true; } -bool exportVisual(Visual &vis, TiXmlElement *xml) +bool exportVisual(Visual &vis, XMLElement *xml) { // // @@ -614,7 +630,9 @@ bool exportVisual(Visual &vis, TiXmlElement *xml) // // // - TiXmlElement * visual_xml = new TiXmlElement("visual"); + XMLDocument * doc = xml->GetDocument(); + + XMLElement * visual_xml = doc->NewElement("visual"); exportPose(vis.origin, visual_xml); @@ -623,12 +641,12 @@ bool exportVisual(Visual &vis, TiXmlElement *xml) if (vis.material) exportMaterial(*vis.material, visual_xml); - xml->LinkEndChild(visual_xml); + xml->InsertEndChild(visual_xml); return true; } -bool exportCollision(Collision &col, TiXmlElement* xml) +bool exportCollision(Collision &col, XMLElement* xml) { // // @@ -637,21 +655,25 @@ bool exportCollision(Collision &col, TiXmlElement* xml) // // // - TiXmlElement * collision_xml = new TiXmlElement("collision"); + XMLDocument * doc = xml->GetDocument(); + + XMLElement * collision_xml = doc->NewElement("collision"); exportPose(col.origin, collision_xml); exportGeometry(col.geometry, collision_xml); - xml->LinkEndChild(collision_xml); + xml->InsertEndChild(collision_xml); return true; } -bool exportLink(Link &link, TiXmlElement* xml) +bool exportLink(Link &link, XMLElement* xml) { - TiXmlElement * link_xml = new TiXmlElement("link"); - link_xml->SetAttribute("name", link.name); + XMLDocument * doc = xml->GetDocument(); + + XMLElement * link_xml = doc->NewElement("link"); + link_xml->SetAttribute("name", link.name.c_str()); if (link.inertial) exportInertial(*link.inertial, link_xml); @@ -660,7 +682,7 @@ 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); + xml->InsertEndChild(link_xml); return true; } diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 2932eadf..0fb32a67 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -41,11 +41,13 @@ #include "urdf_parser/urdf_parser.h" #include +using namespace tinyxml2; + namespace urdf{ -bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); -bool parseLink(Link &link, TiXmlElement *config); -bool parseJoint(Joint &joint, TiXmlElement *config); +bool parseMaterial(Material &material, XMLElement *config, bool only_name_is_ok); +bool parseLink(Link &link, XMLElement *config); +bool parseJoint(Joint &joint, XMLElement *config); ModelInterfaceSharedPtr parseURDFFile(const std::string &path) { @@ -93,17 +95,17 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) ModelInterfaceSharedPtr model(new ModelInterface); model->clear(); - TiXmlDocument xml_doc; + XMLDocument xml_doc; xml_doc.Parse(xml_string.c_str()); if (xml_doc.Error()) { - CONSOLE_BRIDGE_logError(xml_doc.ErrorDesc()); - xml_doc.ClearError(); + CONSOLE_BRIDGE_logError(xml_doc.GetErrorStr1()); + xml_doc.Clear(); model.reset(); return model; } - TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot"); + XMLElement *robot_xml = xml_doc.FirstChildElement("robot"); if (!robot_xml) { CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the xml file"); @@ -137,7 +139,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) } // Get all Material elements - for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) + for (XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) { MaterialSharedPtr material; material.reset(new Material); @@ -166,7 +168,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) } // Get all Link elements - for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) + for (XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) { LinkSharedPtr link; link.reset(new Link); @@ -209,7 +211,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) } // Get all Joint elements - for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) + for (XMLElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) { JointSharedPtr joint; joint.reset(new Joint); @@ -269,16 +271,15 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) return model; } -bool exportMaterial(Material &material, TiXmlElement *config); -bool exportLink(Link &link, TiXmlElement *config); -bool exportJoint(Joint &joint, TiXmlElement *config); -TiXmlDocument* exportURDF(const ModelInterface &model) +bool exportMaterial(Material &material, XMLElement *config); +bool exportLink(Link &link, XMLElement *config); +bool exportJoint(Joint &joint, XMLElement *config); +void exportURDF(const ModelInterface &model, XMLDocument &doc) { - TiXmlDocument *doc = new TiXmlDocument(); - TiXmlElement *robot = new TiXmlElement("robot"); - robot->SetAttribute("name", model.name_); - doc->LinkEndChild(robot); + XMLElement *robot = doc.NewElement("robot"); + robot->SetAttribute("name", model.name_.c_str()); + doc.InsertEndChild(robot); for (std::map::const_iterator m=model.materials_.begin(); m!=model.materials_.end(); ++m) @@ -299,12 +300,11 @@ TiXmlDocument* exportURDF(const ModelInterface &model) exportJoint(*(j->second), robot); } - return doc; } -TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model) +void exportURDF(ModelInterfaceSharedPtr &model, XMLDocument &doc) { - return exportURDF(*model); + exportURDF(*model, doc); } diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp index 56bedd48..bccd5bc4 100644 --- a/urdf_parser/src/pose.cpp +++ b/urdf_parser/src/pose.cpp @@ -40,9 +40,11 @@ #include #include #include -#include +#include #include +using namespace tinyxml2; + namespace urdf_export_helpers { std::string values2str(unsigned int count, const double *values, double (*conv)(double)) @@ -87,7 +89,7 @@ std::string values2str(double d) namespace urdf{ -bool parsePose(Pose &pose, TiXmlElement* xml) +bool parsePose(Pose &pose, XMLElement* xml) { pose.clear(); if (xml) @@ -119,14 +121,15 @@ bool parsePose(Pose &pose, TiXmlElement* xml) return true; } -bool exportPose(Pose &pose, TiXmlElement* xml) +bool exportPose(Pose &pose, XMLElement* xml) { - TiXmlElement *origin = new TiXmlElement("origin"); + XMLDocument *doc = xml->GetDocument(); + XMLElement *origin = doc->NewElement("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()); + xml->InsertEndChild(origin); return true; } diff --git a/urdf_parser/src/twist.cpp b/urdf_parser/src/twist.cpp index 694fc914..c6824fe2 100644 --- a/urdf_parser/src/twist.cpp +++ b/urdf_parser/src/twist.cpp @@ -39,12 +39,14 @@ #include #include #include -#include +#include #include +using namespace tinyxml2; + namespace urdf{ -bool parseTwist(Twist &twist, TiXmlElement* xml) +bool parseTwist(Twist &twist, XMLElement* xml) { twist.clear(); if (xml) diff --git a/urdf_parser/src/urdf_model_state.cpp b/urdf_parser/src/urdf_model_state.cpp index 40169781..89eef6f8 100644 --- a/urdf_parser/src/urdf_model_state.cpp +++ b/urdf_parser/src/urdf_model_state.cpp @@ -43,13 +43,12 @@ #include #include #include -#include +#include #include - #include namespace urdf{ -bool parseModelState(ModelState &ms, TiXmlElement* config) +bool parseModelState(ModelState &ms, XMLElement* config) { ms.clear(); @@ -72,7 +71,7 @@ bool parseModelState(ModelState &ms, TiXmlElement* config) } } - TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state"); + XMLElement *joint_state_elem = config->FirstChildElement("joint_state"); if (joint_state_elem) { JointStateSharedPtr joint_state; diff --git a/urdf_parser/src/urdf_sensor.cpp b/urdf_parser/src/urdf_sensor.cpp index ee863944..c49e3e1e 100644 --- a/urdf_parser/src/urdf_sensor.cpp +++ b/urdf_parser/src/urdf_sensor.cpp @@ -42,21 +42,20 @@ #include #include #include -#include +#include #include - #include namespace urdf{ -bool parsePose(Pose &pose, TiXmlElement* xml); +bool parsePose(Pose &pose, XMLElement* xml); -bool parseCamera(Camera &camera, TiXmlElement* config) +bool parseCamera(Camera &camera, XMLElement* config) { camera.clear(); camera.type = VisualSensor::CAMERA; - TiXmlElement *image = config->FirstChildElement("image"); + XMLElement *image = config->FirstChildElement("image"); if (image) { const char* width_char = image->Attribute("width"); @@ -173,12 +172,12 @@ bool parseCamera(Camera &camera, TiXmlElement* config) return true; } -bool parseRay(Ray &ray, TiXmlElement* config) +bool parseRay(Ray &ray, XMLElement* config) { ray.clear(); ray.type = VisualSensor::RAY; - TiXmlElement *horizontal = config->FirstChildElement("horizontal"); + XMLElement *horizontal = config->FirstChildElement("horizontal"); if (horizontal) { const char* samples_char = horizontal->Attribute("samples"); @@ -234,7 +233,7 @@ bool parseRay(Ray &ray, TiXmlElement* config) } } - TiXmlElement *vertical = config->FirstChildElement("vertical"); + XMLElement *vertical = config->FirstChildElement("vertical"); if (vertical) { const char* samples_char = vertical->Attribute("samples"); @@ -292,12 +291,12 @@ bool parseRay(Ray &ray, TiXmlElement* config) return false; } -VisualSensorSharedPtr parseVisualSensor(TiXmlElement *g) +VisualSensorSharedPtr parseVisualSensor(XMLElement *g) { VisualSensorSharedPtr visual_sensor; // get sensor type - TiXmlElement *sensor_xml; + XMLElement *sensor_xml; if (g->FirstChildElement("camera")) { Camera *camera = new Camera(); @@ -322,7 +321,7 @@ VisualSensorSharedPtr parseVisualSensor(TiXmlElement *g) } -bool parseSensor(Sensor &sensor, TiXmlElement* config) +bool parseSensor(Sensor &sensor, XMLElement* config) { sensor.clear(); @@ -344,7 +343,7 @@ bool parseSensor(Sensor &sensor, TiXmlElement* config) sensor.parent_link_name = std::string(parent_link_name_char); // parse origin - TiXmlElement *o = config->FirstChildElement("origin"); + XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(sensor.origin, o)) diff --git a/urdf_parser/src/world.cpp b/urdf_parser/src/world.cpp index ce09c1c3..903451a9 100644 --- a/urdf_parser/src/world.cpp +++ b/urdf_parser/src/world.cpp @@ -41,12 +41,13 @@ #include #include #include -#include #include +using namespace tinyxml2; + namespace urdf{ -bool parseWorld(World &/*world*/, TiXmlElement* /*config*/) +bool parseWorld(World &/*world*/, XMLElement* /*config*/) { // to be implemented @@ -54,15 +55,16 @@ bool parseWorld(World &/*world*/, TiXmlElement* /*config*/) return true; } -bool exportWorld(World &world, TiXmlElement* xml) +bool exportWorld(World &world, XMLElement* xml) { - TiXmlElement * world_xml = new TiXmlElement("world"); - world_xml->SetAttribute("name", world.name); + XMLDocument * doc = xml->GetDocument(); + XMLElement * world_xml = doc->NewElement("world"); + world_xml->SetAttribute("name", world.name.c_str()); // to be implemented // exportModels(*world.models, world_xml); - xml->LinkEndChild(world_xml); + xml->InsertEndChild(world_xml); return true; } From 5ba4be45097ca65bcc1ed0cce3de586cc557e1cf Mon Sep 17 00:00:00 2001 From: Sean Molenaar Date: Tue, 17 Jan 2023 10:39:43 +0100 Subject: [PATCH 2/8] chore: rebase --- .github/workflows/system.yml | 2 +- package.xml | 8 ++++---- urdf_parser/src/link.cpp | 2 +- urdf_parser/src/model.cpp | 2 +- urdf_parser/src/urdf_model_state.cpp | 3 +++ urdf_parser/src/urdf_sensor.cpp | 2 ++ 6 files changed, 12 insertions(+), 7 deletions(-) diff --git a/.github/workflows/system.yml b/.github/workflows/system.yml index c255647a..cd7749b1 100644 --- a/.github/workflows/system.yml +++ b/.github/workflows/system.yml @@ -44,7 +44,7 @@ jobs: run: | sudo apt-get -qq update sudo apt-get -qq upgrade -y - sudo apt-get -qq install -y build-essential cmake libconsole-bridge-dev liburdfdom-headers-dev libtinyxml-dev + sudo apt-get -qq install -y build-essential cmake libconsole-bridge-dev liburdfdom-headers-dev libtinyxml2-dev cmake --version gcc --version diff --git a/package.xml b/package.xml index c88973d5..16af4c87 100644 --- a/package.xml +++ b/package.xml @@ -17,16 +17,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 python3 diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index 61f00ac9..b2cd6003 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -320,7 +320,7 @@ bool parseInertial(Inertial &i, XMLElement *config) for (auto& attr : attrs) { - if (!inertia_xml->Attribute(attr.first)) + if (!inertia_xml->Attribute(attr.first.c_str())) { std::stringstream stm; stm << "Inertial: inertia element missing " << attr.first << " attribute"; diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 0fb32a67..f75474a7 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -99,7 +99,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) xml_doc.Parse(xml_string.c_str()); if (xml_doc.Error()) { - CONSOLE_BRIDGE_logError(xml_doc.GetErrorStr1()); + CONSOLE_BRIDGE_logError(xml_doc.ErrorStr()); xml_doc.Clear(); model.reset(); return model; diff --git a/urdf_parser/src/urdf_model_state.cpp b/urdf_parser/src/urdf_model_state.cpp index 89eef6f8..a0346bd6 100644 --- a/urdf_parser/src/urdf_model_state.cpp +++ b/urdf_parser/src/urdf_model_state.cpp @@ -46,6 +46,9 @@ #include #include #include + +using namespace tinyxml2; + namespace urdf{ bool parseModelState(ModelState &ms, XMLElement* config) diff --git a/urdf_parser/src/urdf_sensor.cpp b/urdf_parser/src/urdf_sensor.cpp index c49e3e1e..51411f79 100644 --- a/urdf_parser/src/urdf_sensor.cpp +++ b/urdf_parser/src/urdf_sensor.cpp @@ -46,6 +46,8 @@ #include #include +using namespace tinyxml2; + namespace urdf{ bool parsePose(Pose &pose, XMLElement* xml); From b9aebf1b5a5f847e68e4f78cd0fa421a768afa62 Mon Sep 17 00:00:00 2001 From: Sean Molenaar Date: Wed, 18 Jan 2023 15:23:07 +0100 Subject: [PATCH 3/8] convert world.cpp to conditional tinyxml2 --- .github/workflows/system.yml | 2 +- CMakeLists.txt | 22 ++++-- cmake/FindTinyXML.cmake | 74 +++++++++++++++++++ package.xml | 4 + urdf_parser/CMakeLists.txt | 3 + urdf_parser/include/urdf_parser/urdf_parser.h | 15 +++- urdf_parser/src/world.cpp | 22 +++++- 7 files changed, 132 insertions(+), 10 deletions(-) create mode 100644 cmake/FindTinyXML.cmake diff --git a/.github/workflows/system.yml b/.github/workflows/system.yml index cd7749b1..abc2f868 100644 --- a/.github/workflows/system.yml +++ b/.github/workflows/system.yml @@ -44,7 +44,7 @@ jobs: run: | sudo apt-get -qq update sudo apt-get -qq upgrade -y - sudo apt-get -qq install -y build-essential cmake libconsole-bridge-dev liburdfdom-headers-dev libtinyxml2-dev + sudo apt-get -qq install -y build-essential cmake libconsole-bridge-dev liburdfdom-headers-dev libtinyxml-dev libtinyxml2-dev cmake --version gcc --version diff --git a/CMakeLists.txt b/CMakeLists.txt index 93f01cdd..39a2b911 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,15 +45,23 @@ endif (MSVC OR MSVC90 OR MSVC10) set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") -find_package(TinyXML2 REQUIRED) -include_directories(SYSTEM ${TinyXML2_INCLUDE_DIRS}) -link_libraries(${TinyXML2_LIBRARIES}) +find_package(tinyxml_vendor QUIET) +find_package(TinyXML) +# bionic has not cmake module, workaround +if (NOT TinyXML_FOUND AND UNIX) + include(FindPkgConfig) + pkg_check_modules (TinyXML tinyxml) +else() + # Make it fail in platforms without pkgconfig + find_package(TinyXML REQUIRED) # bionic has not cmake module +endif() -find_package(urdfdom_headers 1.0 REQUIRED) -include_directories(SYSTEM ${urdfdom_headers_INCLUDE_DIRS}) -if (NOT MSVC) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +find_package(TinyXML2) +if (TinyXML2_FOUND) + include_directories(SYSTEM ${TinyXML2_INCLUDE_DIRS}) + link_libraries(${TinyXML2_LIBRARIES}) endif() + find_package(urdfdom_headers 1.0 REQUIRED) find_package(console_bridge_vendor QUIET) # Provides console_bridge 0.4.0 on platforms without it. find_package(console_bridge REQUIRED) diff --git a/cmake/FindTinyXML.cmake b/cmake/FindTinyXML.cmake new file mode 100644 index 00000000..fecfefa5 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/package.xml b/package.xml index 16af4c87..e745f192 100644 --- a/package.xml +++ b/package.xml @@ -17,7 +17,9 @@ console_bridge_vendor libconsole-bridge-dev + tinyxml tinyxml2 + tinyxml_vendor tinyxml2_vendor urdfdom_headers @@ -25,7 +27,9 @@ console_bridge_vendor libconsole-bridge-dev + tinyxml tinyxml2 + tinyxml_vendor tinyxml2_vendor urdfdom_headers diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt index ac7c0792..5c3ee674 100644 --- a/urdf_parser/CMakeLists.txt +++ b/urdf_parser/CMakeLists.txt @@ -21,6 +21,9 @@ macro(add_urdfdom_library) set_target_properties(${add_urdfdom_library_LIBNAME} PROPERTIES DEFINE_SYMBOL URDFDOM_EXPORTS SOVERSION ${URDF_MAJOR_MINOR_VERSION}) + if (TinyXML2_FOUND) + target_compile_definitions(${add_urdfdom_library_LIBNAME} PUBLIC HAVE_TINYXML2) + endif() endmacro() if(TARGET console_bridge::console_bridge) diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index eaf4c871..fbfdf7f2 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -40,8 +40,11 @@ #include #include #include -#include +#ifdef HAVE_TINYXML2 #include +#else +#include +#endif #include #include #include @@ -143,6 +146,7 @@ namespace urdf{ URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string); URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string &path); +#ifdef HAVE_TINYXML2 URDFDOM_DLLAPI tinyxml2::XMLElement* exportURDF(ModelInterfaceSharedPtr &model); URDFDOM_DLLAPI tinyxml2::XMLElement* exportURDF(const ModelInterface &model); URDFDOM_DLLAPI bool parsePose(Pose&, tinyxml2::XMLElement*); @@ -150,6 +154,15 @@ namespace urdf{ URDFDOM_DLLAPI bool parseRay(Ray&, tinyxml2::XMLElement*); URDFDOM_DLLAPI bool parseSensor(Sensor&, tinyxml2::XMLElement*); URDFDOM_DLLAPI bool parseModelState(ModelState&, tinyxml2::XMLElement*); +#else + URDFDOM_DLLAPI TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model); + URDFDOM_DLLAPI TiXmlDocument* exportURDF(const ModelInterface &model); + URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*); + URDFDOM_DLLAPI bool parseCamera(Camera&, TiXmlElement*); + URDFDOM_DLLAPI bool parseRay(Ray&, TiXmlElement*); + URDFDOM_DLLAPI bool parseSensor(Sensor&, TiXmlElement*); + URDFDOM_DLLAPI bool parseModelState(ModelState&, TiXmlElement*); +#endif } #endif diff --git a/urdf_parser/src/world.cpp b/urdf_parser/src/world.cpp index 903451a9..7173f7c9 100644 --- a/urdf_parser/src/world.cpp +++ b/urdf_parser/src/world.cpp @@ -43,11 +43,20 @@ #include #include +#ifdef HAVE_TINYXML2 +#include using namespace tinyxml2; +#else +#include +#endif namespace urdf{ +#ifdef HAVE_TINYXML2 bool parseWorld(World &/*world*/, XMLElement* /*config*/) +#else +bool parseWorld(World &/*world*/, TiXmlElement* /*config*/) +#endif { // to be implemented @@ -55,6 +64,7 @@ bool parseWorld(World &/*world*/, XMLElement* /*config*/) return true; } +#ifdef HAVE_TINYXML2 bool exportWorld(World &world, XMLElement* xml) { XMLDocument * doc = xml->GetDocument(); @@ -65,8 +75,18 @@ bool exportWorld(World &world, XMLElement* xml) // exportModels(*world.models, world_xml); xml->InsertEndChild(world_xml); +#else +bool exportWorld(World &world, TiXmlElement* xml) +{ + TiXmlElement * world_xml = new TiXmlElement("world"); + world_xml->SetAttribute("name", world.name); + + // to be implemented + // exportModels(*world.models, world_xml); + + xml->LinkEndChild(world_xml); +#endif return true; } - } From a65ae220196640c00be23f9a4eb9ada3b4858926 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 6 Feb 2023 01:11:39 -0800 Subject: [PATCH 4/8] Add DISABLE_TINYXML_SUPPORT cmake option Require tinyxml2 and allow tinyxml support to be disabled. Signed-off-by: Steve Peters --- CMakeLists.txt | 32 +++++++++++-------- urdf_parser/CMakeLists.txt | 17 ++++++---- urdf_parser/include/urdf_parser/urdf_parser.h | 22 ++++++------- 3 files changed, 38 insertions(+), 33 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 39a2b911..cb05ef8c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,10 @@ if(APPEND_PROJECT_NAME_TO_INCLUDEDIR) set(CMAKE_INSTALL_INCLUDEDIR "${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME}") endif() +option(DISABLE_TINYXML_SUPPORT + "When ON, only tinyxml2 will be found and linked against. \ + When OFF, both tinyxml and tinyxml2 will be found and linked against." OFF) + # set the default build type if (NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) @@ -45,22 +49,22 @@ endif (MSVC OR MSVC90 OR MSVC10) set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") -find_package(tinyxml_vendor QUIET) -find_package(TinyXML) -# bionic has not cmake module, workaround -if (NOT TinyXML_FOUND AND UNIX) - include(FindPkgConfig) - pkg_check_modules (TinyXML tinyxml) -else() - # Make it fail in platforms without pkgconfig - find_package(TinyXML REQUIRED) # bionic has not cmake module +if(DISABLE_TINYXML_SUPPORT) + find_package(tinyxml_vendor QUIET) + find_package(TinyXML) + # bionic has not cmake module, workaround + if (NOT TinyXML_FOUND AND UNIX) + include(FindPkgConfig) + pkg_check_modules (TinyXML tinyxml) + else() + # Make it fail in platforms without pkgconfig + find_package(TinyXML REQUIRED) # bionic has not cmake module + endif() endif() -find_package(TinyXML2) -if (TinyXML2_FOUND) - include_directories(SYSTEM ${TinyXML2_INCLUDE_DIRS}) - link_libraries(${TinyXML2_LIBRARIES}) -endif() +find_package(TinyXML2 REQUIRED) +include_directories(SYSTEM ${TinyXML2_INCLUDE_DIRS}) +link_libraries(${TinyXML2_LIBRARIES}) find_package(urdfdom_headers 1.0 REQUIRED) find_package(console_bridge_vendor QUIET) # Provides console_bridge 0.4.0 on platforms without it. diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt index 5c3ee674..5a45d090 100644 --- a/urdf_parser/CMakeLists.txt +++ b/urdf_parser/CMakeLists.txt @@ -5,25 +5,28 @@ macro(add_urdfdom_library) add_library(${add_urdfdom_library_LIBNAME} SHARED ${add_urdfdom_library_SOURCES}) - target_include_directories(${add_urdfdom_library_LIBNAME} SYSTEM PUBLIC - ${TinyXML_INCLUDE_DIRS}) target_include_directories(${add_urdfdom_library_LIBNAME} PUBLIC "$" - "$") + "$" + ${TinyXML2_INCLUDE_DIR}) target_link_libraries(${add_urdfdom_library_LIBNAME} PUBLIC ${add_urdfdom_library_LINK} ${console_bridge_link_libs} ${urdfdom_headers_link_libs} - ${TinyXML_LIBRARIES}) + ${TinyXML2_LIBRARY}) + if(NOT DISABLE_TINYXML_SUPPORT) + target_include_directories(${add_urdfdom_library_LIBNAME} SYSTEM PUBLIC + ${TinyXML_INCLUDE_DIRS}) + target_link_libraries(${add_urdfdom_library_LIBNAME} PUBLIC + ${TinyXML_LIBRARIES}) + target_compile_definitions(${add_urdfdom_library_LIBNAME} PUBLIC HAVE_TINYXML) + endif() if(NOT CMAKE_CXX_STANDARD) target_compile_features(${add_urdfdom_library_LIBNAME} PUBLIC cxx_std_14) endif() set_target_properties(${add_urdfdom_library_LIBNAME} PROPERTIES DEFINE_SYMBOL URDFDOM_EXPORTS SOVERSION ${URDF_MAJOR_MINOR_VERSION}) - if (TinyXML2_FOUND) - target_compile_definitions(${add_urdfdom_library_LIBNAME} PUBLIC HAVE_TINYXML2) - endif() endmacro() if(TARGET console_bridge::console_bridge) diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index fbfdf7f2..5a483848 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -40,11 +40,10 @@ #include #include #include -#ifdef HAVE_TINYXML2 -#include -#else +#ifdef HAVE_TINYXML #include #endif +#include #include #include #include @@ -146,15 +145,7 @@ namespace urdf{ URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string); URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string &path); -#ifdef HAVE_TINYXML2 - URDFDOM_DLLAPI tinyxml2::XMLElement* exportURDF(ModelInterfaceSharedPtr &model); - URDFDOM_DLLAPI tinyxml2::XMLElement* exportURDF(const ModelInterface &model); - URDFDOM_DLLAPI bool parsePose(Pose&, tinyxml2::XMLElement*); - URDFDOM_DLLAPI bool parseCamera(Camera&, tinyxml2::XMLElement*); - URDFDOM_DLLAPI bool parseRay(Ray&, tinyxml2::XMLElement*); - URDFDOM_DLLAPI bool parseSensor(Sensor&, tinyxml2::XMLElement*); - URDFDOM_DLLAPI bool parseModelState(ModelState&, tinyxml2::XMLElement*); -#else +#ifdef HAVE_TINYXML URDFDOM_DLLAPI TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model); URDFDOM_DLLAPI TiXmlDocument* exportURDF(const ModelInterface &model); URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*); @@ -163,6 +154,13 @@ namespace urdf{ URDFDOM_DLLAPI bool parseSensor(Sensor&, TiXmlElement*); URDFDOM_DLLAPI bool parseModelState(ModelState&, TiXmlElement*); #endif + URDFDOM_DLLAPI void exportURDF(ModelInterfaceSharedPtr &model, XMLDocument &doc); + URDFDOM_DLLAPI void exportURDF(const ModelInterface &model, XMLDocument &doc); + URDFDOM_DLLAPI bool parsePose(Pose&, tinyxml2::XMLElement*); + URDFDOM_DLLAPI bool parseCamera(Camera&, tinyxml2::XMLElement*); + URDFDOM_DLLAPI bool parseRay(Ray&, tinyxml2::XMLElement*); + URDFDOM_DLLAPI bool parseSensor(Sensor&, tinyxml2::XMLElement*); + URDFDOM_DLLAPI bool parseModelState(ModelState&, tinyxml2::XMLElement*); } #endif From 0bca4966ee00b8f097af0b945b3250a0f4fed4ca Mon Sep 17 00:00:00 2001 From: Sean Molenaar Date: Wed, 8 Feb 2023 20:01:45 +0100 Subject: [PATCH 5/8] urdf_parser: convert remaining files to HAVE_TINYXML --- CMakeLists.txt | 2 +- urdf_parser/include/urdf_parser/urdf_parser.h | 4 +- urdf_parser/src/joint.cpp | 599 ++++++++++++++++- urdf_parser/src/link.cpp | 620 ++++++++++++++++++ urdf_parser/src/model.cpp | 46 +- urdf_parser/src/pose.cpp | 48 ++ urdf_parser/src/twist.cpp | 38 ++ urdf_parser/src/urdf_model_state.cpp | 101 +++ urdf_parser/src/urdf_sensor.cpp | 312 +++++++++ urdf_parser/src/world.cpp | 30 +- 10 files changed, 1782 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cb05ef8c..df886208 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,7 +49,7 @@ endif (MSVC OR MSVC90 OR MSVC10) set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") -if(DISABLE_TINYXML_SUPPORT) +if(NOT DISABLE_TINYXML_SUPPORT) find_package(tinyxml_vendor QUIET) find_package(TinyXML) # bionic has not cmake module, workaround diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index 5a483848..567f5a34 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -154,8 +154,8 @@ namespace urdf{ URDFDOM_DLLAPI bool parseSensor(Sensor&, TiXmlElement*); URDFDOM_DLLAPI bool parseModelState(ModelState&, TiXmlElement*); #endif - URDFDOM_DLLAPI void exportURDF(ModelInterfaceSharedPtr &model, XMLDocument &doc); - URDFDOM_DLLAPI void exportURDF(const ModelInterface &model, XMLDocument &doc); + URDFDOM_DLLAPI void exportURDF(ModelInterfaceSharedPtr &model, tinyxml2::XMLDocument &doc); + URDFDOM_DLLAPI void exportURDF(const ModelInterface &model, tinyxml2::XMLDocument &doc); URDFDOM_DLLAPI bool parsePose(Pose&, tinyxml2::XMLElement*); URDFDOM_DLLAPI bool parseCamera(Camera&, tinyxml2::XMLElement*); URDFDOM_DLLAPI bool parseRay(Ray&, tinyxml2::XMLElement*); diff --git a/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp index 78aee4f9..845466ec 100644 --- a/urdf_parser/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -39,9 +39,13 @@ #include #include #include +#include #include + +#ifdef HAVE_TINYXML +#include +#endif #include -#include using namespace tinyxml2; @@ -653,4 +657,597 @@ bool exportJoint(Joint &joint, XMLElement* xml) +#ifdef HAVE_TINYXML +bool parsePose(Pose &pose, TiXmlElement* xml); + +bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) +{ + jd.clear(); + + // Get joint damping + const char* damping_str = config->Attribute("damping"); + if (damping_str == NULL){ + CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no damping, defaults to 0"); + jd.damping = 0; + } + else + { + try { + jd.damping = strToDouble(damping_str); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("damping value (%s) is not a valid float", damping_str); + return false; + } + } + + // Get joint friction + const char* friction_str = config->Attribute("friction"); + if (friction_str == NULL){ + CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no friction, defaults to 0"); + jd.friction = 0; + } + else + { + try { + jd.friction = strToDouble(friction_str); + } catch (std::runtime_error &) { + CONSOLE_BRIDGE_logError("friction value (%s) is not a valid float", friction_str); + return false; + } + } + + if (damping_str == NULL && friction_str == NULL) + { + CONSOLE_BRIDGE_logError("joint dynamics element specified with no damping and no friction"); + return false; + } + else{ + CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: damping %f and friction %f", jd.damping, jd.friction); + return true; + } +} + +bool parseJointLimits(JointLimits &jl, TiXmlElement* config) +{ + jl.clear(); + + // Get lower joint limit + const char* lower_str = config->Attribute("lower"); + if (lower_str == NULL){ + CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no lower, defaults to 0"); + jl.lower = 0; + } + else + { + try { + jl.lower = strToDouble(lower_str); + } catch (std::runtime_error &) { + CONSOLE_BRIDGE_logError("lower value (%s) is not a valid float", lower_str); + return false; + } + } + + // Get upper joint limit + const char* upper_str = config->Attribute("upper"); + if (upper_str == NULL){ + CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no upper, , defaults to 0"); + jl.upper = 0; + } + else + { + try { + jl.upper = strToDouble(upper_str); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("upper value (%s) is not a valid float", upper_str); + return false; + } + } + + // Get joint effort limit + const char* effort_str = config->Attribute("effort"); + if (effort_str == NULL){ + CONSOLE_BRIDGE_logError("joint limit: no effort"); + return false; + } + else + { + try { + jl.effort = strToDouble(effort_str); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("effort value (%s) is not a valid float", effort_str); + return false; + } + } + + // Get joint velocity limit + const char* velocity_str = config->Attribute("velocity"); + if (velocity_str == NULL){ + CONSOLE_BRIDGE_logError("joint limit: no velocity"); + return false; + } + else + { + try { + jl.velocity = strToDouble(velocity_str); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("velocity value (%s) is not a valid float", velocity_str); + return false; + } + } + + return true; +} + +bool parseJointSafety(JointSafety &js, TiXmlElement* config) +{ + js.clear(); + + // Get soft_lower_limit joint limit + const char* soft_lower_limit_str = config->Attribute("soft_lower_limit"); + if (soft_lower_limit_str == NULL) + { + CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_lower_limit, using default value"); + js.soft_lower_limit = 0; + } + else + { + try { + js.soft_lower_limit = strToDouble(soft_lower_limit_str); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("soft_lower_limit value (%s) is not a valid float", soft_lower_limit_str); + return false; + } + } + + // Get soft_upper_limit joint limit + const char* soft_upper_limit_str = config->Attribute("soft_upper_limit"); + if (soft_upper_limit_str == NULL) + { + CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_upper_limit, using default value"); + js.soft_upper_limit = 0; + } + else + { + try { + js.soft_upper_limit = strToDouble(soft_upper_limit_str); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("soft_upper_limit value (%s) is not a valid float", soft_upper_limit_str); + return false; + } + } + + // Get k_position_ safety "position" gain - not exactly position gain + const char* k_position_str = config->Attribute("k_position"); + if (k_position_str == NULL) + { + CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no k_position, using default value"); + js.k_position = 0; + } + else + { + try { + js.k_position = strToDouble(k_position_str); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("k_position value (%s) is not a valid float", k_position_str); + return false; + } + } + // Get k_velocity_ safety velocity gain + const char* k_velocity_str = config->Attribute("k_velocity"); + if (k_velocity_str == NULL) + { + CONSOLE_BRIDGE_logError("joint safety: no k_velocity"); + return false; + } + else + { + try { + js.k_velocity = strToDouble(k_velocity_str); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("k_velocity value (%s) is not a valid float", k_velocity_str); + return false; + } + } + + return true; +} + +bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config) +{ + jc.clear(); + + // Get rising edge position + const char* rising_position_str = config->Attribute("rising"); + if (rising_position_str == NULL) + { + CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no rising, using default value"); + jc.rising.reset(); + } + else + { + try { + jc.rising.reset(new double(strToDouble(rising_position_str))); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("rising value (%s) is not a valid float", rising_position_str); + return false; + } + } + + // Get falling edge position + const char* falling_position_str = config->Attribute("falling"); + if (falling_position_str == NULL) + { + CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no falling, using default value"); + jc.falling.reset(); + } + else + { + try { + jc.falling.reset(new double(strToDouble(falling_position_str))); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("falling value (%s) is not a valid float", falling_position_str); + return false; + } + } + + return true; +} + +bool parseJointMimic(JointMimic &jm, TiXmlElement* config) +{ + jm.clear(); + + // Get name of joint to mimic + const char* joint_name_str = config->Attribute("joint"); + + if (joint_name_str == NULL) + { + CONSOLE_BRIDGE_logError("joint mimic: no mimic joint specified"); + return false; + } + else + jm.joint_name = joint_name_str; + + // Get mimic multiplier + const char* multiplier_str = config->Attribute("multiplier"); + + if (multiplier_str == NULL) + { + CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no multiplier, using default value of 1"); + jm.multiplier = 1; + } + else + { + try { + jm.multiplier = strToDouble(multiplier_str); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("multiplier value (%s) is not a valid float", multiplier_str); + return false; + } + } + + + // Get mimic offset + const char* offset_str = config->Attribute("offset"); + if (offset_str == NULL) + { + CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no offset, using default value of 0"); + jm.offset = 0; + } + else + { + try { + jm.offset = strToDouble(offset_str); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("offset value (%s) is not a valid float", offset_str); + return false; + } + } + + return true; +} + +bool parseJoint(Joint &joint, TiXmlElement* config) +{ + joint.clear(); + + // Get Joint Name + const char *name = config->Attribute("name"); + if (!name) + { + CONSOLE_BRIDGE_logError("unnamed joint found"); + return false; + } + joint.name = name; + + // Get transform from Parent Link to Joint Frame + TiXmlElement *origin_xml = config->FirstChildElement("origin"); + if (!origin_xml) + { + CONSOLE_BRIDGE_logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str()); + joint.parent_to_joint_origin_transform.clear(); + } + else + { + if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml)) + { + joint.parent_to_joint_origin_transform.clear(); + CONSOLE_BRIDGE_logError("Malformed parent origin element for joint [%s]", joint.name.c_str()); + return false; + } + } + + // Get Parent Link + TiXmlElement *parent_xml = config->FirstChildElement("parent"); + if (parent_xml) + { + const char *pname = parent_xml->Attribute("link"); + if (!pname) + { + CONSOLE_BRIDGE_logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str()); + } + else + { + joint.parent_link_name = std::string(pname); + } + } + + // Get Child Link + TiXmlElement *child_xml = config->FirstChildElement("child"); + if (child_xml) + { + const char *pname = child_xml->Attribute("link"); + if (!pname) + { + CONSOLE_BRIDGE_logInform("no child link name specified for Joint link [%s].", joint.name.c_str()); + } + else + { + joint.child_link_name = std::string(pname); + } + } + + // Get Joint type + const char* type_char = config->Attribute("type"); + if (!type_char) + { + CONSOLE_BRIDGE_logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str()); + return false; + } + + std::string type_str = type_char; + if (type_str == "planar") + joint.type = Joint::PLANAR; + else if (type_str == "floating") + joint.type = Joint::FLOATING; + else if (type_str == "revolute") + joint.type = Joint::REVOLUTE; + else if (type_str == "continuous") + joint.type = Joint::CONTINUOUS; + else if (type_str == "prismatic") + joint.type = Joint::PRISMATIC; + else if (type_str == "fixed") + joint.type = Joint::FIXED; + else + { + CONSOLE_BRIDGE_logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str()); + return false; + } + + // Get Joint Axis + if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED) + { + // axis + TiXmlElement *axis_xml = config->FirstChildElement("axis"); + if (!axis_xml){ + CONSOLE_BRIDGE_logDebug("urdfdom: no axis element for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str()); + joint.axis = Vector3(1.0, 0.0, 0.0); + } + else{ + if (axis_xml->Attribute("xyz")){ + try { + joint.axis.init(axis_xml->Attribute("xyz")); + } + catch (ParseError &e) { + joint.axis.clear(); + CONSOLE_BRIDGE_logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what()); + return false; + } + } + } + } + + // Get limit + TiXmlElement *limit_xml = config->FirstChildElement("limit"); + if (limit_xml) + { + joint.limits.reset(new JointLimits()); + if (!parseJointLimits(*joint.limits, limit_xml)) + { + CONSOLE_BRIDGE_logError("Could not parse limit element for joint [%s]", joint.name.c_str()); + joint.limits.reset(); + return false; + } + } + else if (joint.type == Joint::REVOLUTE) + { + CONSOLE_BRIDGE_logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str()); + return false; + } + else if (joint.type == Joint::PRISMATIC) + { + CONSOLE_BRIDGE_logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str()); + return false; + } + + // Get safety + TiXmlElement *safety_xml = config->FirstChildElement("safety_controller"); + if (safety_xml) + { + joint.safety.reset(new JointSafety()); + if (!parseJointSafety(*joint.safety, safety_xml)) + { + CONSOLE_BRIDGE_logError("Could not parse safety element for joint [%s]", joint.name.c_str()); + joint.safety.reset(); + return false; + } + } + + // Get calibration + TiXmlElement *calibration_xml = config->FirstChildElement("calibration"); + if (calibration_xml) + { + joint.calibration.reset(new JointCalibration()); + if (!parseJointCalibration(*joint.calibration, calibration_xml)) + { + CONSOLE_BRIDGE_logError("Could not parse calibration element for joint [%s]", joint.name.c_str()); + joint.calibration.reset(); + return false; + } + } + + // Get Joint Mimic + TiXmlElement *mimic_xml = config->FirstChildElement("mimic"); + if (mimic_xml) + { + joint.mimic.reset(new JointMimic()); + if (!parseJointMimic(*joint.mimic, mimic_xml)) + { + CONSOLE_BRIDGE_logError("Could not parse mimic element for joint [%s]", joint.name.c_str()); + joint.mimic.reset(); + return false; + } + } + + // Get Dynamics + TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); + if (prop_xml) + { + joint.dynamics.reset(new JointDynamics()); + if (!parseJointDynamics(*joint.dynamics, prop_xml)) + { + CONSOLE_BRIDGE_logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str()); + joint.dynamics.reset(); + return false; + } + } + + return true; +} + +/* exports */ +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); + 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); + 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); + return true; +} + +bool exportJointCalibration(JointCalibration &jc, TiXmlElement* xml) +{ + if (jc.falling || jc.rising) + { + TiXmlElement *calibration_xml = new TiXmlElement("calibration"); + if (jc.falling) + calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling) ); + if (jc.rising) + calibration_xml->SetAttribute("rising", urdf_export_helpers::values2str(*jc.rising) ); + //calibration_xml->SetAttribute("reference_position", urdf_export_helpers::values2str(jc.reference_position) ); + xml->LinkEndChild(calibration_xml); + } + return true; +} + +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); + } + return true; +} + +bool exportJoint(Joint &joint, TiXmlElement* xml) +{ + TiXmlElement * joint_xml = new TiXmlElement("joint"); + joint_xml->SetAttribute("name", joint.name); + if (joint.type == urdf::Joint::PLANAR) + joint_xml->SetAttribute("type", "planar"); + else if (joint.type == urdf::Joint::FLOATING) + joint_xml->SetAttribute("type", "floating"); + else if (joint.type == urdf::Joint::REVOLUTE) + joint_xml->SetAttribute("type", "revolute"); + else if (joint.type == urdf::Joint::CONTINUOUS) + joint_xml->SetAttribute("type", "continuous"); + else if (joint.type == urdf::Joint::PRISMATIC) + joint_xml->SetAttribute("type", "prismatic"); + else if (joint.type == urdf::Joint::FIXED) + joint_xml->SetAttribute("type", "fixed"); + else + CONSOLE_BRIDGE_logError("ERROR: Joint [%s] type [%d] is not a defined type.\n",joint.name.c_str(), joint.type); + + // origin + 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); + + // parent + TiXmlElement * parent_xml = new TiXmlElement("parent"); + parent_xml->SetAttribute("link", joint.parent_link_name); + joint_xml->LinkEndChild(parent_xml); + + // child + TiXmlElement * child_xml = new TiXmlElement("child"); + child_xml->SetAttribute("link", joint.child_link_name); + joint_xml->LinkEndChild(child_xml); + + if (joint.dynamics) + exportJointDynamics(*(joint.dynamics), joint_xml); + if (joint.limits) + exportJointLimits(*(joint.limits), joint_xml); + if (joint.safety) + exportJointSafety(*(joint.safety), joint_xml); + if (joint.calibration) + exportJointCalibration(*(joint.calibration), joint_xml); + if (joint.mimic) + exportJointMimic(*(joint.mimic), joint_xml); + + xml->LinkEndChild(joint_xml); + return true; +} +#endif + } diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index b2cd6003..7c67f010 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -45,6 +45,9 @@ #include #include #include +#ifdef HAVE_TINYXML +#include +#endif #include #include @@ -687,4 +690,621 @@ bool exportLink(Link &link, XMLElement* xml) return true; } +#ifdef HAVE_TINYXML +bool parsePose(Pose &pose, TiXmlElement* xml); + +bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok) +{ + bool has_rgb = false; + bool has_filename = false; + + material.clear(); + + if (!config->Attribute("name")) + { + CONSOLE_BRIDGE_logError("Material must contain a name attribute"); + return false; + } + + material.name = config->Attribute("name"); + + // texture + TiXmlElement *t = config->FirstChildElement("texture"); + if (t) + { + if (t->Attribute("filename")) + { + material.texture_filename = t->Attribute("filename"); + has_filename = true; + } + } + + // color + TiXmlElement *c = config->FirstChildElement("color"); + if (c) + { + if (c->Attribute("rgba")) { + + try { + material.color.init(c->Attribute("rgba")); + has_rgb = true; + } + catch (ParseError &e) { + material.color.clear(); + CONSOLE_BRIDGE_logError(std::string("Material [" + material.name + "] has malformed color rgba values: " + e.what()).c_str()); + } + } + } + + if (!has_rgb && !has_filename) { + if (!only_name_is_ok) // no need for an error if only name is ok + { + CONSOLE_BRIDGE_logError(std::string("Material ["+material.name+"] color has no rgba").c_str()); + CONSOLE_BRIDGE_logError(std::string("Material ["+material.name+"] not defined in file").c_str()); + } + return false; + } + return true; +} + + +bool parseSphere(Sphere &s, TiXmlElement *c) +{ + s.clear(); + + s.type = Geometry::SPHERE; + if (!c->Attribute("radius")) + { + CONSOLE_BRIDGE_logError("Sphere shape must have a radius attribute"); + return false; + } + + try { + s.radius = strToDouble(c->Attribute("radius")); + } catch(std::runtime_error &) { + std::stringstream stm; + stm << "radius [" << c->Attribute("radius") << "] is not a valid float"; + CONSOLE_BRIDGE_logError(stm.str().c_str()); + return false; + } + + return true; +} + +bool parseBox(Box &b, TiXmlElement *c) +{ + b.clear(); + + b.type = Geometry::BOX; + if (!c->Attribute("size")) + { + CONSOLE_BRIDGE_logError("Box shape has no size attribute"); + return false; + } + try + { + b.dim.init(c->Attribute("size")); + } + catch (ParseError &e) + { + b.dim.clear(); + CONSOLE_BRIDGE_logError(e.what()); + return false; + } + return true; +} + +bool parseCylinder(Cylinder &y, TiXmlElement *c) +{ + y.clear(); + + y.type = Geometry::CYLINDER; + if (!c->Attribute("length") || + !c->Attribute("radius")) + { + CONSOLE_BRIDGE_logError("Cylinder shape must have both length and radius attributes"); + return false; + } + + try { + y.length = strToDouble(c->Attribute("length")); + } catch(std::runtime_error &) { + std::stringstream stm; + stm << "length [" << c->Attribute("length") << "] is not a valid float"; + CONSOLE_BRIDGE_logError(stm.str().c_str()); + return false; + } + + try { + y.radius = strToDouble(c->Attribute("radius")); + } catch(std::runtime_error &) { + std::stringstream stm; + stm << "radius [" << c->Attribute("radius") << "] is not a valid float"; + CONSOLE_BRIDGE_logError(stm.str().c_str()); + return false; + } + + return true; +} + + +bool parseMesh(Mesh &m, TiXmlElement *c) +{ + m.clear(); + + m.type = Geometry::MESH; + if (!c->Attribute("filename")) { + CONSOLE_BRIDGE_logError("Mesh must contain a filename attribute"); + return false; + } + + m.filename = c->Attribute("filename"); + + if (c->Attribute("scale")) { + try { + m.scale.init(c->Attribute("scale")); + } + catch (ParseError &e) { + m.scale.clear(); + CONSOLE_BRIDGE_logError("Mesh scale was specified, but could not be parsed: %s", e.what()); + return false; + } + } + else + { + m.scale.x = m.scale.y = m.scale.z = 1; + } + return true; +} + +GeometrySharedPtr parseGeometry(TiXmlElement *g) +{ + GeometrySharedPtr geom; + if (!g) return geom; + + TiXmlElement *shape = g->FirstChildElement(); + if (!shape) + { + CONSOLE_BRIDGE_logError("Geometry tag contains no child element."); + return geom; + } + + std::string type_name = shape->ValueStr(); + if (type_name == "sphere") + { + Sphere *s = new Sphere(); + geom.reset(s); + if (parseSphere(*s, shape)) + return geom; + } + else if (type_name == "box") + { + Box *b = new Box(); + geom.reset(b); + if (parseBox(*b, shape)) + return geom; + } + else if (type_name == "cylinder") + { + Cylinder *c = new Cylinder(); + geom.reset(c); + if (parseCylinder(*c, shape)) + return geom; + } + else if (type_name == "mesh") + { + Mesh *m = new Mesh(); + geom.reset(m); + if (parseMesh(*m, shape)) + return geom; + } + else + { + CONSOLE_BRIDGE_logError("Unknown geometry type '%s'", type_name.c_str()); + return geom; + } + + return GeometrySharedPtr(); +} + +bool parseInertial(Inertial &i, TiXmlElement *config) +{ + i.clear(); + + // Origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) + { + if (!parsePose(i.origin, o)) + return false; + } + + TiXmlElement *mass_xml = config->FirstChildElement("mass"); + if (!mass_xml) + { + CONSOLE_BRIDGE_logError("Inertial element must have a mass element"); + return false; + } + if (!mass_xml->Attribute("value")) + { + CONSOLE_BRIDGE_logError("Inertial: mass element must have value attribute"); + return false; + } + + try { + i.mass = strToDouble(mass_xml->Attribute("value")); + } catch(std::runtime_error &) { + std::stringstream stm; + stm << "Inertial: mass [" << mass_xml->Attribute("value") + << "] is not a float"; + CONSOLE_BRIDGE_logError(stm.str().c_str()); + return false; + } + + TiXmlElement *inertia_xml = config->FirstChildElement("inertia"); + if (!inertia_xml) + { + CONSOLE_BRIDGE_logError("Inertial element must have inertia element"); + return false; + } + + std::vector> attrs{ + std::make_pair("ixx", 0.0), + std::make_pair("ixy", 0.0), + std::make_pair("ixz", 0.0), + std::make_pair("iyy", 0.0), + std::make_pair("iyz", 0.0), + std::make_pair("izz", 0.0) + }; + + for (auto& attr : attrs) + { + if (!inertia_xml->Attribute(attr.first)) + { + std::stringstream stm; + stm << "Inertial: inertia element missing " << attr.first << " attribute"; + CONSOLE_BRIDGE_logError(stm.str().c_str()); + return false; + } + + try { + attr.second = strToDouble(inertia_xml->Attribute(attr.first.c_str())); + } catch(std::runtime_error &) { + std::stringstream stm; + stm << "Inertial: inertia element " << attr.first << " is not a valid double"; + CONSOLE_BRIDGE_logError(stm.str().c_str()); + return false; + } + } + + i.ixx = attrs[0].second; + i.ixy = attrs[1].second; + i.ixz = attrs[2].second; + i.iyy = attrs[3].second; + i.iyz = attrs[4].second; + i.izz = attrs[5].second; + + return true; +} + +bool parseVisual(Visual &vis, TiXmlElement *config) +{ + vis.clear(); + + // Origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) { + if (!parsePose(vis.origin, o)) + return false; + } + + // Geometry + TiXmlElement *geom = config->FirstChildElement("geometry"); + vis.geometry = parseGeometry(geom); + if (!vis.geometry) + return false; + + const char *name_char = config->Attribute("name"); + if (name_char) + vis.name = name_char; + + // Material + TiXmlElement *mat = config->FirstChildElement("material"); + if (mat) { + // get material name + if (!mat->Attribute("name")) { + CONSOLE_BRIDGE_logError("Visual material must contain a name attribute"); + return false; + } + vis.material_name = mat->Attribute("name"); + + // try to parse material element in place + vis.material.reset(new Material()); + if (!parseMaterial(*vis.material, mat, true)) + { + vis.material.reset(); + } + } + + return true; +} + +bool parseCollision(Collision &col, TiXmlElement* config) +{ + col.clear(); + + // Origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) { + if (!parsePose(col.origin, o)) + return false; + } + + // Geometry + TiXmlElement *geom = config->FirstChildElement("geometry"); + col.geometry = parseGeometry(geom); + if (!col.geometry) + return false; + + const char *name_char = config->Attribute("name"); + if (name_char) + col.name = name_char; + + return true; +} + +bool parseLink(Link &link, TiXmlElement* config) +{ + + link.clear(); + + const char *name_char = config->Attribute("name"); + if (!name_char) + { + CONSOLE_BRIDGE_logError("No name given for the link."); + return false; + } + link.name = std::string(name_char); + + // Inertial (optional) + TiXmlElement *i = config->FirstChildElement("inertial"); + if (i) + { + link.inertial.reset(new Inertial()); + if (!parseInertial(*link.inertial, i)) + { + CONSOLE_BRIDGE_logError("Could not parse inertial element for Link [%s]", link.name.c_str()); + return false; + } + } + + // Multiple Visuals (optional) + for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) + { + + VisualSharedPtr vis; + vis.reset(new Visual()); + if (parseVisual(*vis, vis_xml)) + { + link.visual_array.push_back(vis); + } + else + { + vis.reset(); + CONSOLE_BRIDGE_logError("Could not parse visual element for Link [%s]", link.name.c_str()); + return false; + } + } + + // Visual (optional) + // Assign the first visual to the .visual ptr, if it exists + if (!link.visual_array.empty()) + link.visual = link.visual_array[0]; + + // Multiple Collisions (optional) + for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) + { + CollisionSharedPtr col; + col.reset(new Collision()); + if (parseCollision(*col, col_xml)) + { + link.collision_array.push_back(col); + } + else + { + col.reset(); + CONSOLE_BRIDGE_logError("Could not parse collision element for Link [%s]", link.name.c_str()); + return false; + } + } + + // Collision (optional) + // Assign the first collision to the .collision ptr, if it exists + if (!link.collision_array.empty()) + link.collision = link.collision_array[0]; + + return true; +} + +/* exports */ +bool exportPose(Pose &pose, TiXmlElement* xml); + +bool exportMaterial(Material &material, TiXmlElement *xml) +{ + TiXmlElement *material_xml = new TiXmlElement("material"); + material_xml->SetAttribute("name", material.name); + + TiXmlElement* texture = new TiXmlElement("texture"); + if (!material.texture_filename.empty()) + texture->SetAttribute("filename", material.texture_filename); + material_xml->LinkEndChild(texture); + + TiXmlElement* color = new TiXmlElement("color"); + color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color)); + material_xml->LinkEndChild(color); + xml->LinkEndChild(material_xml); + 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); + 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); + 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); + return true; +} + +bool exportMesh(Mesh &m, TiXmlElement *xml) +{ + // e.g. add + TiXmlElement *mesh_xml = new TiXmlElement("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); + return true; +} + +bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml) +{ + TiXmlElement *geometry_xml = new TiXmlElement("geometry"); + if (urdf::dynamic_pointer_cast(geom)) + { + exportSphere((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); + } + else if (urdf::dynamic_pointer_cast(geom)) + { + exportBox((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); + } + else if (urdf::dynamic_pointer_cast(geom)) + { + exportCylinder((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); + } + else if (urdf::dynamic_pointer_cast(geom)) + { + exportMesh((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); + } + else + { + CONSOLE_BRIDGE_logError("geometry not specified, I'll make one up for you!"); + Sphere *s = new Sphere(); + s->radius = 0.03; + geom.reset(s); + exportSphere((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); + } + + xml->LinkEndChild(geometry_xml); + return true; +} + +bool exportInertial(Inertial &i, TiXmlElement *xml) +{ + // adds + // + // + // + // + TiXmlElement *inertial_xml = new TiXmlElement("inertial"); + + TiXmlElement *mass_xml = new TiXmlElement("mass"); + mass_xml->SetAttribute("value", urdf_export_helpers::values2str(i.mass)); + inertial_xml->LinkEndChild(mass_xml); + + 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); + + return true; +} + +bool exportVisual(Visual &vis, TiXmlElement *xml) +{ + // + // + // + // + // + // + // + TiXmlElement * visual_xml = new TiXmlElement("visual"); + + exportPose(vis.origin, visual_xml); + + exportGeometry(vis.geometry, visual_xml); + + if (vis.material) + exportMaterial(*vis.material, visual_xml); + + xml->LinkEndChild(visual_xml); + + return true; +} + +bool exportCollision(Collision &col, TiXmlElement* xml) +{ + // + // + // + // + // + // + // + TiXmlElement * collision_xml = new TiXmlElement("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); + + if (link.inertial) + exportInertial(*link.inertial, link_xml); + for (std::size_t i = 0 ; i < link.visual_array.size() ; ++i) + exportVisual(*link.visual_array[i], link_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; +} +#endif + } diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index f75474a7..a2d3cec0 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -90,7 +90,7 @@ bool assignMaterial(const VisualSharedPtr& visual, ModelInterfaceSharedPtr& mode return true; } -ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) +ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) { ModelInterfaceSharedPtr model(new ModelInterface); model->clear(); @@ -274,7 +274,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) bool exportMaterial(Material &material, XMLElement *config); bool exportLink(Link &link, XMLElement *config); bool exportJoint(Joint &joint, XMLElement *config); -void exportURDF(const ModelInterface &model, XMLDocument &doc) +void exportURDF(const ModelInterface &model, XMLDocument &doc) { XMLElement *robot = doc.NewElement("robot"); @@ -306,7 +306,49 @@ void exportURDF(ModelInterfaceSharedPtr &model, XMLDocument &doc) { exportURDF(*model, doc); } +#ifdef HAVE_TINYXML +bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); +bool parseLink(Link &link, TiXmlElement *config); +bool parseJoint(Joint &joint, TiXmlElement *config); +bool exportMaterial(Material &material, TiXmlElement *config); +bool exportLink(Link &link, TiXmlElement *config); +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); + + + for (std::map::const_iterator m=model.materials_.begin(); m!=model.materials_.end(); ++m) + { + CONSOLE_BRIDGE_logDebug("urdfdom: exporting material [%s]\n",m->second->name.c_str()); + exportMaterial(*(m->second), robot); + } + + for (std::map::const_iterator l=model.links_.begin(); l!=model.links_.end(); ++l) + { + CONSOLE_BRIDGE_logDebug("urdfdom: exporting link [%s]\n",l->second->name.c_str()); + exportLink(*(l->second), robot); + } + + for (std::map::const_iterator j=model.joints_.begin(); j!=model.joints_.end(); ++j) + { + CONSOLE_BRIDGE_logDebug("urdfdom: exporting joint [%s]\n",j->second->name.c_str()); + exportJoint(*(j->second), robot); + } + + return doc; +} + +TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model) +{ + return exportURDF(*model); +} +#endif } diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp index bccd5bc4..4a07dadc 100644 --- a/urdf_parser/src/pose.cpp +++ b/urdf_parser/src/pose.cpp @@ -40,6 +40,9 @@ #include #include #include +#ifdef HAVE_TINYXML +#include +#endif #include #include @@ -133,6 +136,51 @@ bool exportPose(Pose &pose, XMLElement* xml) return true; } +#ifdef HAVE_TINYXML +bool parsePose(Pose &pose, TiXmlElement* xml) +{ + pose.clear(); + if (xml) + { + const char* xyz_str = xml->Attribute("xyz"); + if (xyz_str != NULL) + { + try { + pose.position.init(xyz_str); + } + catch (ParseError &e) { + CONSOLE_BRIDGE_logError(e.what()); + return false; + } + } + + const char* rpy_str = xml->Attribute("rpy"); + if (rpy_str != NULL) + { + try { + pose.rotation.init(rpy_str); + } + catch (ParseError &e) { + CONSOLE_BRIDGE_logError(e.what()); + return false; + } + } + } + return true; +} + +bool exportPose(Pose &pose, TiXmlElement* xml) +{ + TiXmlElement *origin = new TiXmlElement("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); + return true; +} + +#endif } diff --git a/urdf_parser/src/twist.cpp b/urdf_parser/src/twist.cpp index c6824fe2..98ada76e 100644 --- a/urdf_parser/src/twist.cpp +++ b/urdf_parser/src/twist.cpp @@ -39,6 +39,9 @@ #include #include #include +#ifdef HAVE_TINYXML +#include +#endif #include #include @@ -80,6 +83,41 @@ bool parseTwist(Twist &twist, XMLElement* xml) return true; } +#ifdef HAVE_TINYXML +bool parseTwist(Twist &twist, TiXmlElement* xml) +{ + twist.clear(); + if (xml) + { + const char* linear_char = xml->Attribute("linear"); + if (linear_char != NULL) + { + try { + twist.linear.init(linear_char); + } + catch (ParseError &e) { + twist.linear.clear(); + CONSOLE_BRIDGE_logError("Malformed linear string [%s]: %s", linear_char, e.what()); + return false; + } + } + + const char* angular_char = xml->Attribute("angular"); + if (angular_char != NULL) + { + try { + twist.angular.init(angular_char); + } + catch (ParseError &e) { + twist.angular.clear(); + CONSOLE_BRIDGE_logError("Malformed angular [%s]: %s", angular_char, e.what()); + return false; + } + } + } + return true; +} +#endif } diff --git a/urdf_parser/src/urdf_model_state.cpp b/urdf_parser/src/urdf_model_state.cpp index a0346bd6..a7d2ad69 100644 --- a/urdf_parser/src/urdf_model_state.cpp +++ b/urdf_parser/src/urdf_model_state.cpp @@ -43,6 +43,9 @@ #include #include #include +#ifdef HAVE_TINYXML +#include +#endif #include #include #include @@ -149,7 +152,105 @@ bool parseModelState(ModelState &ms, XMLElement* config) return false; } +#ifdef HAVE_TINYXML +bool parseModelState(ModelState &ms, TiXmlElement* config) +{ + ms.clear(); + + const char *name_char = config->Attribute("name"); + if (!name_char) + { + CONSOLE_BRIDGE_logError("No name given for the model_state."); + return false; + } + ms.name = std::string(name_char); + + const char *time_stamp_char = config->Attribute("time_stamp"); + if (time_stamp_char) + { + try { + ms.time_stamp.set(strToDouble(time_stamp_char)); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("Parsing time stamp [%s] failed", time_stamp_char); + return false; + } + } + + TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state"); + if (joint_state_elem) + { + JointStateSharedPtr joint_state; + joint_state.reset(new JointState()); + + const char *joint_char = joint_state_elem->Attribute("joint"); + if (joint_char) + joint_state->joint = std::string(joint_char); + else + { + CONSOLE_BRIDGE_logError("No joint name given for the model_state."); + return false; + } + + // parse position + const char *position_char = joint_state_elem->Attribute("position"); + if (position_char) + { + + std::vector pieces; + urdf::split_string( pieces, position_char, " "); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + joint_state->position.push_back(strToDouble(pieces[i].c_str())); + } catch(std::runtime_error &) { + throw ParseError("position element ("+ pieces[i] +") is not a valid float"); + } + } + } + } + + // parse velocity + const char *velocity_char = joint_state_elem->Attribute("velocity"); + if (velocity_char) + { + + std::vector pieces; + urdf::split_string( pieces, velocity_char, " "); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + joint_state->velocity.push_back(strToDouble(pieces[i].c_str())); + } catch(std::runtime_error &) { + throw ParseError("velocity element ("+ pieces[i] +") is not a valid float"); + } + } + } + } + // parse effort + const char *effort_char = joint_state_elem->Attribute("effort"); + if (effort_char) + { + + std::vector pieces; + urdf::split_string( pieces, effort_char, " "); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + try { + joint_state->effort.push_back(strToDouble(pieces[i].c_str())); + } catch(std::runtime_error &) { + throw ParseError("effort element ("+ pieces[i] +") is not a valid float"); + } + } + } + } + + // add to vector + ms.joint_states.push_back(joint_state); + } + return false; +} +#endif } diff --git a/urdf_parser/src/urdf_sensor.cpp b/urdf_parser/src/urdf_sensor.cpp index 51411f79..60cb6da0 100644 --- a/urdf_parser/src/urdf_sensor.cpp +++ b/urdf_parser/src/urdf_sensor.cpp @@ -42,6 +42,9 @@ #include #include #include +#ifdef HAVE_TINYXML +#include +#endif #include #include #include @@ -357,6 +360,315 @@ bool parseSensor(Sensor &sensor, XMLElement* config) return true; } +#ifdef HAVE_TINYXML +bool parsePose(Pose &pose, TiXmlElement* xml); + +bool parseCamera(Camera &camera, TiXmlElement* config) +{ + camera.clear(); + camera.type = VisualSensor::CAMERA; + + TiXmlElement *image = config->FirstChildElement("image"); + if (image) + { + const char* width_char = image->Attribute("width"); + if (width_char) + { + try + { + camera.width = std::stoul(width_char); + } + catch (std::invalid_argument &e) + { + CONSOLE_BRIDGE_logError("Camera image width [%s] is not a valid int: %s", width_char, e.what()); + return false; + } + catch (std::out_of_range &e) + { + CONSOLE_BRIDGE_logError("Camera image width [%s] is out of range: %s", width_char, e.what()); + return false; + } + } + else + { + CONSOLE_BRIDGE_logError("Camera sensor needs an image width attribute"); + return false; + } + + const char* height_char = image->Attribute("height"); + if (height_char) + { + try + { + camera.height = std::stoul(height_char); + } + catch (std::invalid_argument &e) + { + CONSOLE_BRIDGE_logError("Camera image height [%s] is not a valid int: %s", height_char, e.what()); + return false; + } + catch (std::out_of_range &e) + { + CONSOLE_BRIDGE_logError("Camera image height [%s] is out of range: %s", height_char, e.what()); + return false; + } + } + else + { + CONSOLE_BRIDGE_logError("Camera sensor needs an image height attribute"); + return false; + } + + const char* format_char = image->Attribute("format"); + if (format_char) + camera.format = std::string(format_char); + else + { + CONSOLE_BRIDGE_logError("Camera sensor needs an image format attribute"); + return false; + } + + const char* hfov_char = image->Attribute("hfov"); + if (hfov_char) + { + try { + camera.hfov = strToDouble(hfov_char); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("Camera image hfov [%s] is not a valid float", hfov_char); + return false; + } + } + else + { + CONSOLE_BRIDGE_logError("Camera sensor needs an image hfov attribute"); + return false; + } + + const char* near_char = image->Attribute("near"); + if (near_char) + { + try { + camera.near = strToDouble(near_char); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("Camera image near [%s] is not a valid float", near_char); + return false; + } + } + else + { + CONSOLE_BRIDGE_logError("Camera sensor needs an image near attribute"); + return false; + } + + const char* far_char = image->Attribute("far"); + if (far_char) + { + try { + camera.far = strToDouble(far_char); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("Camera image far [%s] is not a valid float", far_char); + return false; + } + } + else + { + CONSOLE_BRIDGE_logError("Camera sensor needs an image far attribute"); + return false; + } + + } + else + { + CONSOLE_BRIDGE_logError("Camera sensor has no element"); + return false; + } + return true; +} + +bool parseRay(Ray &ray, TiXmlElement* config) +{ + ray.clear(); + ray.type = VisualSensor::RAY; + + TiXmlElement *horizontal = config->FirstChildElement("horizontal"); + if (horizontal) + { + const char* samples_char = horizontal->Attribute("samples"); + if (samples_char) + { + try + { + ray.horizontal_samples = std::stoul(samples_char); + } + catch (std::invalid_argument &e) + { + CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what()); + return false; + } + catch (std::out_of_range &e) + { + CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is out of range: %s", samples_char, e.what()); + return false; + } + } + + const char* resolution_char = horizontal->Attribute("resolution"); + if (resolution_char) + { + try { + ray.horizontal_resolution = strToDouble(resolution_char); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("Ray horizontal resolution [%s] is not a valid float", resolution_char); + return false; + } + } + + const char* min_angle_char = horizontal->Attribute("min_angle"); + if (min_angle_char) + { + try { + ray.horizontal_min_angle = strToDouble(min_angle_char); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("Ray horizontal min_angle [%s] is not a valid float", min_angle_char); + return false; + } + } + + const char* max_angle_char = horizontal->Attribute("max_angle"); + if (max_angle_char) + { + try { + ray.horizontal_max_angle = strToDouble(max_angle_char); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("Ray horizontal max_angle [%s] is not a valid float", max_angle_char); + return false; + } + } + } + + TiXmlElement *vertical = config->FirstChildElement("vertical"); + if (vertical) + { + const char* samples_char = vertical->Attribute("samples"); + if (samples_char) + { + try + { + ray.vertical_samples = std::stoul(samples_char); + } + catch (std::invalid_argument &e) + { + CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what()); + return false; + } + catch (std::out_of_range &e) + { + CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is out of range: %s", samples_char, e.what()); + return false; + } + } + + const char* resolution_char = vertical->Attribute("resolution"); + if (resolution_char) + { + try { + ray.vertical_resolution = strToDouble(resolution_char); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("Ray vertical resolution [%s] is not a valid float", resolution_char); + return false; + } + } + + const char* min_angle_char = vertical->Attribute("min_angle"); + if (min_angle_char) + { + try { + ray.vertical_min_angle = strToDouble(min_angle_char); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("Ray vertical min_angle [%s] is not a valid float", min_angle_char); + return false; + } + } + + const char* max_angle_char = vertical->Attribute("max_angle"); + if (max_angle_char) + { + try { + ray.vertical_max_angle = strToDouble(max_angle_char); + } catch(std::runtime_error &) { + CONSOLE_BRIDGE_logError("Ray vertical max_angle [%s] is not a valid float", max_angle_char); + return false; + } + } + } + return false; +} + +VisualSensorSharedPtr parseVisualSensor(TiXmlElement *g) +{ + VisualSensorSharedPtr visual_sensor; + + // get sensor type + TiXmlElement *sensor_xml; + if (g->FirstChildElement("camera")) + { + Camera *camera = new Camera(); + visual_sensor.reset(camera); + sensor_xml = g->FirstChildElement("camera"); + if (!parseCamera(*camera, sensor_xml)) + visual_sensor.reset(); + } + else if (g->FirstChildElement("ray")) + { + Ray *ray = new Ray(); + visual_sensor.reset(ray); + sensor_xml = g->FirstChildElement("ray"); + if (!parseRay(*ray, sensor_xml)) + visual_sensor.reset(); + } + else + { + CONSOLE_BRIDGE_logError("No know sensor types [camera|ray] defined in block"); + } + return visual_sensor; +} + + +bool parseSensor(Sensor &sensor, TiXmlElement* config) +{ + sensor.clear(); + + const char *name_char = config->Attribute("name"); + if (!name_char) + { + CONSOLE_BRIDGE_logError("No name given for the sensor."); + return false; + } + sensor.name = std::string(name_char); + + // parse parent_link_name + const char *parent_link_name_char = config->Attribute("parent_link_name"); + if (!parent_link_name_char) + { + CONSOLE_BRIDGE_logError("No parent_link_name given for the sensor."); + return false; + } + sensor.parent_link_name = std::string(parent_link_name_char); + + // parse origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) + { + if (!parsePose(sensor.origin, o)) + return false; + } + + // parse sensor + sensor.sensor = parseVisualSensor(config); + return true; +} + +#endif } diff --git a/urdf_parser/src/world.cpp b/urdf_parser/src/world.cpp index 7173f7c9..f20f768a 100644 --- a/urdf_parser/src/world.cpp +++ b/urdf_parser/src/world.cpp @@ -43,28 +43,22 @@ #include #include -#ifdef HAVE_TINYXML2 -#include -using namespace tinyxml2; -#else +#ifdef HAVE_TINYXML #include #endif +#include + +using namespace tinyxml2; namespace urdf{ -#ifdef HAVE_TINYXML2 bool parseWorld(World &/*world*/, XMLElement* /*config*/) -#else -bool parseWorld(World &/*world*/, TiXmlElement* /*config*/) -#endif { // to be implemented return true; } - -#ifdef HAVE_TINYXML2 bool exportWorld(World &world, XMLElement* xml) { XMLDocument * doc = xml->GetDocument(); @@ -75,7 +69,19 @@ bool exportWorld(World &world, XMLElement* xml) // exportModels(*world.models, world_xml); xml->InsertEndChild(world_xml); -#else + + return true; +} + +#ifdef HAVE_TINYXML +bool parseWorld(World &/*world*/, TiXmlElement* /*config*/) +{ + + // to be implemented + + return true; +} + bool exportWorld(World &world, TiXmlElement* xml) { TiXmlElement * world_xml = new TiXmlElement("world"); @@ -85,8 +91,8 @@ bool exportWorld(World &world, TiXmlElement* xml) // exportModels(*world.models, world_xml); xml->LinkEndChild(world_xml); -#endif return true; } +#endif } From 092b57c20bca37d1d81eaa6da6b08a7bebb04662 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 19 Mar 2023 17:31:28 -0700 Subject: [PATCH 6/8] Remove tinyxml2 header from public API Signed-off-by: Steve Peters --- urdf_parser/CMakeLists.txt | 1 + urdf_parser/include/urdf_parser/urdf_parser.h | 20 +++++++++++-------- urdf_parser/src/model.cpp | 12 ++++++++--- 3 files changed, 22 insertions(+), 11 deletions(-) diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt index 5a45d090..1edf7e93 100644 --- a/urdf_parser/CMakeLists.txt +++ b/urdf_parser/CMakeLists.txt @@ -64,6 +64,7 @@ add_urdfdom_library( LIBNAME urdfdom_sensor SOURCES + src/pose.cpp src/urdf_sensor.cpp LINK urdfdom_model) diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index 567f5a34..4ffc4e10 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -43,7 +43,6 @@ #ifdef HAVE_TINYXML #include #endif -#include #include #include #include @@ -141,6 +140,11 @@ class URDFVersion final } +namespace tinyxml2{ + // Forward declaration for private APIs. + class XMLElement; +} + namespace urdf{ URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string); @@ -154,13 +158,13 @@ namespace urdf{ URDFDOM_DLLAPI bool parseSensor(Sensor&, TiXmlElement*); URDFDOM_DLLAPI bool parseModelState(ModelState&, TiXmlElement*); #endif - URDFDOM_DLLAPI void exportURDF(ModelInterfaceSharedPtr &model, tinyxml2::XMLDocument &doc); - URDFDOM_DLLAPI void exportURDF(const ModelInterface &model, tinyxml2::XMLDocument &doc); - URDFDOM_DLLAPI bool parsePose(Pose&, tinyxml2::XMLElement*); - URDFDOM_DLLAPI bool parseCamera(Camera&, tinyxml2::XMLElement*); - URDFDOM_DLLAPI bool parseRay(Ray&, tinyxml2::XMLElement*); - URDFDOM_DLLAPI bool parseSensor(Sensor&, tinyxml2::XMLElement*); - URDFDOM_DLLAPI bool parseModelState(ModelState&, tinyxml2::XMLElement*); + URDFDOM_DLLAPI void exportURDF(ModelInterfaceSharedPtr &model, std::string &xml_string); + URDFDOM_DLLAPI void exportURDF(const ModelInterface &model, std::string &xml_string); + bool parsePose(Pose&, tinyxml2::XMLElement*); + bool parseCamera(Camera&, tinyxml2::XMLElement*); + bool parseRay(Ray&, tinyxml2::XMLElement*); + bool parseSensor(Sensor&, tinyxml2::XMLElement*); + bool parseModelState(ModelState&, tinyxml2::XMLElement*); } #endif diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index a2d3cec0..a7c545f1 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -40,6 +40,7 @@ #include #include "urdf_parser/urdf_parser.h" #include +#include using namespace tinyxml2; @@ -274,9 +275,10 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) bool exportMaterial(Material &material, XMLElement *config); bool exportLink(Link &link, XMLElement *config); bool exportJoint(Joint &joint, XMLElement *config); -void exportURDF(const ModelInterface &model, XMLDocument &doc) +void exportURDF(const ModelInterface &model, std::string &xml_string) { + XMLDocument doc; XMLElement *robot = doc.NewElement("robot"); robot->SetAttribute("name", model.name_.c_str()); doc.InsertEndChild(robot); @@ -300,11 +302,15 @@ void exportURDF(const ModelInterface &model, XMLDocument &doc) exportJoint(*(j->second), robot); } + XMLPrinter printer; + doc.Print( &printer ); + xml_string = std::string(printer.CStr()); + } -void exportURDF(ModelInterfaceSharedPtr &model, XMLDocument &doc) +void exportURDF(ModelInterfaceSharedPtr &model, std::string &xml_string) { - exportURDF(*model, doc); + exportURDF(*model, xml_string); } #ifdef HAVE_TINYXML bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); From 3e215fc616e5360404645475fd3b9c6b3e117e02 Mon Sep 17 00:00:00 2001 From: Sean Molenaar Date: Tue, 4 Apr 2023 16:55:50 +0200 Subject: [PATCH 7/8] Apply suggestions from code review Co-authored-by: Chris Lalancette --- CMakeLists.txt | 4 ++-- urdf_parser/src/model.cpp | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index df886208..c5d204b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,13 +52,13 @@ set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") if(NOT DISABLE_TINYXML_SUPPORT) find_package(tinyxml_vendor QUIET) find_package(TinyXML) - # bionic has not cmake module, workaround + # bionic has no cmake module, workaround if (NOT TinyXML_FOUND AND UNIX) include(FindPkgConfig) pkg_check_modules (TinyXML tinyxml) else() # Make it fail in platforms without pkgconfig - find_package(TinyXML REQUIRED) # bionic has not cmake module + find_package(TinyXML REQUIRED) # bionic has no cmake module endif() endif() diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index a7c545f1..9abfc174 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -312,6 +312,7 @@ void exportURDF(ModelInterfaceSharedPtr &model, std::string &xml_string) { exportURDF(*model, xml_string); } + #ifdef HAVE_TINYXML bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); bool parseLink(Link &link, TiXmlElement *config); From aca82500b26fe9b0f9701fca0d88107a542bd929 Mon Sep 17 00:00:00 2001 From: Sean Molenaar Date: Tue, 18 Apr 2023 13:03:40 +0200 Subject: [PATCH 8/8] Fix comments, don't use tinyxml2 namespace --- CMakeLists.txt | 1 - cmake/FindTinyXML.cmake | 2 +- urdf_parser/src/joint.cpp | 78 +++++++++-------- urdf_parser/src/link.cpp | 122 +++++++++++++-------------- urdf_parser/src/model.cpp | 30 +++---- urdf_parser/src/pose.cpp | 10 +-- urdf_parser/src/twist.cpp | 4 +- urdf_parser/src/urdf_model_state.cpp | 6 +- urdf_parser/src/urdf_sensor.cpp | 22 +++-- urdf_parser/src/world.cpp | 10 +-- 10 files changed, 134 insertions(+), 151 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c5d204b4..ba067938 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,7 +64,6 @@ endif() find_package(TinyXML2 REQUIRED) include_directories(SYSTEM ${TinyXML2_INCLUDE_DIRS}) -link_libraries(${TinyXML2_LIBRARIES}) find_package(urdfdom_headers 1.0 REQUIRED) find_package(console_bridge_vendor QUIET) # Provides console_bridge 0.4.0 on platforms without it. diff --git a/cmake/FindTinyXML.cmake b/cmake/FindTinyXML.cmake index fecfefa5..aabb323d 100644 --- a/cmake/FindTinyXML.cmake +++ b/cmake/FindTinyXML.cmake @@ -71,4 +71,4 @@ unset(TINYXML_FOUND) # ...and unset uppercase variable generated by find_package if(TinyXML_FOUND) set(TinyXML_INCLUDE_DIRS ${TinyXML_INCLUDE_DIR}) set(TinyXML_LIBRARIES ${TinyXML_LIBRARY}) -endif() \ No newline at end of file +endif() diff --git a/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp index 845466ec..5f3a9d20 100644 --- a/urdf_parser/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -47,13 +47,11 @@ #endif #include -using namespace tinyxml2; - namespace urdf{ -bool parsePose(Pose &pose, XMLElement* xml); +bool parsePose(Pose &pose, tinyxml2::XMLElement* xml); -bool parseJointDynamics(JointDynamics &jd, XMLElement* config) +bool parseJointDynamics(JointDynamics &jd, tinyxml2::XMLElement* config) { jd.clear(); @@ -100,7 +98,7 @@ bool parseJointDynamics(JointDynamics &jd, XMLElement* config) } } -bool parseJointLimits(JointLimits &jl, XMLElement* config) +bool parseJointLimits(JointLimits &jl, tinyxml2::XMLElement* config) { jl.clear(); @@ -171,7 +169,7 @@ bool parseJointLimits(JointLimits &jl, XMLElement* config) return true; } -bool parseJointSafety(JointSafety &js, XMLElement* config) +bool parseJointSafety(JointSafety &js, tinyxml2::XMLElement* config) { js.clear(); @@ -245,7 +243,7 @@ bool parseJointSafety(JointSafety &js, XMLElement* config) return true; } -bool parseJointCalibration(JointCalibration &jc, XMLElement* config) +bool parseJointCalibration(JointCalibration &jc, tinyxml2::XMLElement* config) { jc.clear(); @@ -286,7 +284,7 @@ bool parseJointCalibration(JointCalibration &jc, XMLElement* config) return true; } -bool parseJointMimic(JointMimic &jm, XMLElement* config) +bool parseJointMimic(JointMimic &jm, tinyxml2::XMLElement* config) { jm.clear(); @@ -340,7 +338,7 @@ bool parseJointMimic(JointMimic &jm, XMLElement* config) return true; } -bool parseJoint(Joint &joint, XMLElement* config) +bool parseJoint(Joint &joint, tinyxml2::XMLElement* config) { joint.clear(); @@ -354,7 +352,7 @@ bool parseJoint(Joint &joint, XMLElement* config) joint.name = name; // Get transform from Parent Link to Joint Frame - XMLElement *origin_xml = config->FirstChildElement("origin"); + tinyxml2::XMLElement *origin_xml = config->FirstChildElement("origin"); if (!origin_xml) { CONSOLE_BRIDGE_logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str()); @@ -371,7 +369,7 @@ bool parseJoint(Joint &joint, XMLElement* config) } // Get Parent Link - XMLElement *parent_xml = config->FirstChildElement("parent"); + tinyxml2::XMLElement *parent_xml = config->FirstChildElement("parent"); if (parent_xml) { const char *pname = parent_xml->Attribute("link"); @@ -386,7 +384,7 @@ bool parseJoint(Joint &joint, XMLElement* config) } // Get Child Link - XMLElement *child_xml = config->FirstChildElement("child"); + tinyxml2::XMLElement *child_xml = config->FirstChildElement("child"); if (child_xml) { const char *pname = child_xml->Attribute("link"); @@ -431,7 +429,7 @@ bool parseJoint(Joint &joint, XMLElement* config) if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED) { // axis - XMLElement *axis_xml = config->FirstChildElement("axis"); + tinyxml2::XMLElement *axis_xml = config->FirstChildElement("axis"); if (!axis_xml){ CONSOLE_BRIDGE_logDebug("urdfdom: no axis element for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str()); joint.axis = Vector3(1.0, 0.0, 0.0); @@ -451,7 +449,7 @@ bool parseJoint(Joint &joint, XMLElement* config) } // Get limit - XMLElement *limit_xml = config->FirstChildElement("limit"); + tinyxml2::XMLElement *limit_xml = config->FirstChildElement("limit"); if (limit_xml) { joint.limits.reset(new JointLimits()); @@ -474,7 +472,7 @@ bool parseJoint(Joint &joint, XMLElement* config) } // Get safety - XMLElement *safety_xml = config->FirstChildElement("safety_controller"); + tinyxml2::XMLElement *safety_xml = config->FirstChildElement("safety_controller"); if (safety_xml) { joint.safety.reset(new JointSafety()); @@ -487,7 +485,7 @@ bool parseJoint(Joint &joint, XMLElement* config) } // Get calibration - XMLElement *calibration_xml = config->FirstChildElement("calibration"); + tinyxml2::XMLElement *calibration_xml = config->FirstChildElement("calibration"); if (calibration_xml) { joint.calibration.reset(new JointCalibration()); @@ -500,7 +498,7 @@ bool parseJoint(Joint &joint, XMLElement* config) } // Get Joint Mimic - XMLElement *mimic_xml = config->FirstChildElement("mimic"); + tinyxml2::XMLElement *mimic_xml = config->FirstChildElement("mimic"); if (mimic_xml) { joint.mimic.reset(new JointMimic()); @@ -513,7 +511,7 @@ bool parseJoint(Joint &joint, XMLElement* config) } // Get Dynamics - XMLElement *prop_xml = config->FirstChildElement("dynamics"); + tinyxml2::XMLElement *prop_xml = config->FirstChildElement("dynamics"); if (prop_xml) { joint.dynamics.reset(new JointDynamics()); @@ -530,24 +528,24 @@ bool parseJoint(Joint &joint, XMLElement* config) /* exports */ -bool exportPose(Pose &pose, XMLElement* xml); +bool exportPose(Pose &pose, tinyxml2::XMLElement* xml); -bool exportJointDynamics(JointDynamics &jd, XMLElement* xml) +bool exportJointDynamics(JointDynamics &jd, tinyxml2::XMLElement* xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); - XMLElement *dynamics_xml = doc->NewElement("dynamics"); + tinyxml2::XMLElement *dynamics_xml = doc->NewElement("dynamics"); dynamics_xml->SetAttribute("damping", urdf_export_helpers::values2str(jd.damping).c_str() ); dynamics_xml->SetAttribute("friction", urdf_export_helpers::values2str(jd.friction).c_str() ); xml->InsertEndChild(dynamics_xml); return true; } -bool exportJointLimits(JointLimits &jl, XMLElement* xml) +bool exportJointLimits(JointLimits &jl, tinyxml2::XMLElement* xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); - XMLElement *limit_xml = doc->NewElement("limit"); + tinyxml2::XMLElement *limit_xml = doc->NewElement("limit"); limit_xml->SetAttribute("effort", urdf_export_helpers::values2str(jl.effort).c_str() ); limit_xml->SetAttribute("velocity", urdf_export_helpers::values2str(jl.velocity).c_str() ); limit_xml->SetAttribute("lower", urdf_export_helpers::values2str(jl.lower).c_str() ); @@ -556,11 +554,11 @@ bool exportJointLimits(JointLimits &jl, XMLElement* xml) return true; } -bool exportJointSafety(JointSafety &js, XMLElement* xml) +bool exportJointSafety(JointSafety &js, tinyxml2::XMLElement* xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); - XMLElement *safety_xml = doc->NewElement("safety_controller"); + tinyxml2::XMLElement *safety_xml = doc->NewElement("safety_controller"); safety_xml->SetAttribute("k_position", urdf_export_helpers::values2str(js.k_position).c_str() ); safety_xml->SetAttribute("k_velocity", urdf_export_helpers::values2str(js.k_velocity).c_str() ); safety_xml->SetAttribute("soft_lower_limit", urdf_export_helpers::values2str(js.soft_lower_limit).c_str() ); @@ -569,13 +567,13 @@ bool exportJointSafety(JointSafety &js, XMLElement* xml) return true; } -bool exportJointCalibration(JointCalibration &jc, XMLElement* xml) +bool exportJointCalibration(JointCalibration &jc, tinyxml2::XMLElement* xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); if (jc.falling || jc.rising) { - XMLElement *calibration_xml = doc->NewElement("calibration"); + tinyxml2::XMLElement *calibration_xml = doc->NewElement("calibration"); if (jc.falling) calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling).c_str() ); if (jc.rising) @@ -586,13 +584,13 @@ bool exportJointCalibration(JointCalibration &jc, XMLElement* xml) return true; } -bool exportJointMimic(JointMimic &jm, XMLElement* xml) +bool exportJointMimic(JointMimic &jm, tinyxml2::XMLElement* xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); if (!jm.joint_name.empty()) { - XMLElement *mimic_xml = doc->NewElement("mimic"); + tinyxml2::XMLElement *mimic_xml = doc->NewElement("mimic"); mimic_xml->SetAttribute("offset", urdf_export_helpers::values2str(jm.offset).c_str() ); mimic_xml->SetAttribute("multiplier", urdf_export_helpers::values2str(jm.multiplier).c_str() ); mimic_xml->SetAttribute("joint", jm.joint_name.c_str() ); @@ -601,11 +599,11 @@ bool exportJointMimic(JointMimic &jm, XMLElement* xml) return true; } -bool exportJoint(Joint &joint, XMLElement* xml) +bool exportJoint(Joint &joint, tinyxml2::XMLElement* xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); - XMLElement * joint_xml = doc->NewElement("joint"); + tinyxml2::XMLElement * joint_xml = doc->NewElement("joint"); joint_xml->SetAttribute("name", joint.name.c_str()); if (joint.type == urdf::Joint::PLANAR) joint_xml->SetAttribute("type", "planar"); @@ -626,17 +624,17 @@ bool exportJoint(Joint &joint, XMLElement* xml) exportPose(joint.parent_to_joint_origin_transform, joint_xml); // axis - XMLElement * axis_xml = doc->NewElement("axis"); + tinyxml2::XMLElement * axis_xml = doc->NewElement("axis"); axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(joint.axis).c_str()); joint_xml->InsertEndChild(axis_xml); // parent - XMLElement * parent_xml = doc-> NewElement("parent"); + tinyxml2::XMLElement * parent_xml = doc-> NewElement("parent"); parent_xml->SetAttribute("link", joint.parent_link_name.c_str()); joint_xml->LinkEndChild(parent_xml); // child - XMLElement * child_xml = doc->NewElement("child"); + tinyxml2::XMLElement * child_xml = doc->NewElement("child"); child_xml->SetAttribute("link", joint.child_link_name.c_str()); joint_xml->InsertEndChild(child_xml); diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index 7c67f010..690c3433 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -51,13 +51,11 @@ #include #include -using namespace tinyxml2; - namespace urdf{ -bool parsePose(Pose &pose, XMLElement* xml); +bool parsePose(Pose &pose, tinyxml2::XMLElement* xml); -bool parseMaterial(Material &material, XMLElement *config, bool only_name_is_ok) +bool parseMaterial(Material &material, tinyxml2::XMLElement *config, bool only_name_is_ok) { bool has_rgb = false; bool has_filename = false; @@ -73,7 +71,7 @@ bool parseMaterial(Material &material, XMLElement *config, bool only_name_is_ok) material.name = config->Attribute("name"); // texture - XMLElement *t = config->FirstChildElement("texture"); + tinyxml2::XMLElement *t = config->FirstChildElement("texture"); if (t) { if (t->Attribute("filename")) @@ -84,7 +82,7 @@ bool parseMaterial(Material &material, XMLElement *config, bool only_name_is_ok) } // color - XMLElement *c = config->FirstChildElement("color"); + tinyxml2::XMLElement *c = config->FirstChildElement("color"); if (c) { if (c->Attribute("rgba")) { @@ -112,7 +110,7 @@ bool parseMaterial(Material &material, XMLElement *config, bool only_name_is_ok) } -bool parseSphere(Sphere &s, XMLElement *c) +bool parseSphere(Sphere &s, tinyxml2::XMLElement *c) { s.clear(); @@ -135,7 +133,7 @@ bool parseSphere(Sphere &s, XMLElement *c) return true; } -bool parseBox(Box &b, XMLElement *c) +bool parseBox(Box &b, tinyxml2::XMLElement *c) { b.clear(); @@ -158,7 +156,7 @@ bool parseBox(Box &b, XMLElement *c) return true; } -bool parseCylinder(Cylinder &y, XMLElement *c) +bool parseCylinder(Cylinder &y, tinyxml2::XMLElement *c) { y.clear(); @@ -192,7 +190,7 @@ bool parseCylinder(Cylinder &y, XMLElement *c) } -bool parseMesh(Mesh &m, XMLElement *c) +bool parseMesh(Mesh &m, tinyxml2::XMLElement *c) { m.clear(); @@ -221,12 +219,12 @@ bool parseMesh(Mesh &m, XMLElement *c) return true; } -GeometrySharedPtr parseGeometry(XMLElement *g) +GeometrySharedPtr parseGeometry(tinyxml2::XMLElement *g) { GeometrySharedPtr geom; if (!g) return geom; - XMLElement *shape = g->FirstChildElement(); + tinyxml2::XMLElement *shape = g->FirstChildElement(); if (!shape) { CONSOLE_BRIDGE_logError("Geometry tag contains no child element."); @@ -271,19 +269,19 @@ GeometrySharedPtr parseGeometry(XMLElement *g) return GeometrySharedPtr(); } -bool parseInertial(Inertial &i, XMLElement *config) +bool parseInertial(Inertial &i, tinyxml2::XMLElement *config) { i.clear(); // Origin - XMLElement *o = config->FirstChildElement("origin"); + tinyxml2::XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(i.origin, o)) return false; } - XMLElement *mass_xml = config->FirstChildElement("mass"); + tinyxml2::XMLElement *mass_xml = config->FirstChildElement("mass"); if (!mass_xml) { CONSOLE_BRIDGE_logError("Inertial element must have a mass element"); @@ -305,7 +303,7 @@ bool parseInertial(Inertial &i, XMLElement *config) return false; } - XMLElement *inertia_xml = config->FirstChildElement("inertia"); + tinyxml2::XMLElement *inertia_xml = config->FirstChildElement("inertia"); if (!inertia_xml) { CONSOLE_BRIDGE_logError("Inertial element must have inertia element"); @@ -351,19 +349,19 @@ bool parseInertial(Inertial &i, XMLElement *config) return true; } -bool parseVisual(Visual &vis, XMLElement *config) +bool parseVisual(Visual &vis, tinyxml2::XMLElement *config) { vis.clear(); // Origin - XMLElement *o = config->FirstChildElement("origin"); + tinyxml2::XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(vis.origin, o)) return false; } // Geometry - XMLElement *geom = config->FirstChildElement("geometry"); + tinyxml2::XMLElement *geom = config->FirstChildElement("geometry"); vis.geometry = parseGeometry(geom); if (!vis.geometry) return false; @@ -373,7 +371,7 @@ bool parseVisual(Visual &vis, XMLElement *config) vis.name = name_char; // Material - XMLElement *mat = config->FirstChildElement("material"); + tinyxml2::XMLElement *mat = config->FirstChildElement("material"); if (mat) { // get material name if (!mat->Attribute("name")) { @@ -393,19 +391,19 @@ bool parseVisual(Visual &vis, XMLElement *config) return true; } -bool parseCollision(Collision &col, XMLElement* config) +bool parseCollision(Collision &col, tinyxml2::XMLElement* config) { col.clear(); // Origin - XMLElement *o = config->FirstChildElement("origin"); + tinyxml2::XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(col.origin, o)) return false; } // Geometry - XMLElement *geom = config->FirstChildElement("geometry"); + tinyxml2::XMLElement *geom = config->FirstChildElement("geometry"); col.geometry = parseGeometry(geom); if (!col.geometry) return false; @@ -417,7 +415,7 @@ bool parseCollision(Collision &col, XMLElement* config) return true; } -bool parseLink(Link &link, XMLElement* config) +bool parseLink(Link &link, tinyxml2::XMLElement* config) { link.clear(); @@ -431,7 +429,7 @@ bool parseLink(Link &link, XMLElement* config) link.name = std::string(name_char); // Inertial (optional) - XMLElement *i = config->FirstChildElement("inertial"); + tinyxml2::XMLElement *i = config->FirstChildElement("inertial"); if (i) { link.inertial.reset(new Inertial()); @@ -443,7 +441,7 @@ bool parseLink(Link &link, XMLElement* config) } // Multiple Visuals (optional) - for (XMLElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) + for (tinyxml2::XMLElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) { VisualSharedPtr vis; @@ -466,7 +464,7 @@ bool parseLink(Link &link, XMLElement* config) link.visual = link.visual_array[0]; // Multiple Collisions (optional) - for (XMLElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) + for (tinyxml2::XMLElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) { CollisionSharedPtr col; col.reset(new Collision()); @@ -491,67 +489,67 @@ bool parseLink(Link &link, XMLElement* config) } /* exports */ -bool exportPose(Pose &pose, XMLElement* xml); +bool exportPose(Pose &pose, tinyxml2::XMLElement* xml); -bool exportMaterial(Material &material, XMLElement *xml) +bool exportMaterial(Material &material, tinyxml2::XMLElement *xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); - XMLElement *material_xml = doc->NewElement("material"); + tinyxml2::XMLElement *material_xml = doc->NewElement("material"); material_xml->SetAttribute("name", material.name.c_str()); - XMLElement* texture = doc->NewElement("texture"); + tinyxml2::XMLElement* texture = doc->NewElement("texture"); if (!material.texture_filename.empty()) texture->SetAttribute("filename", material.texture_filename.c_str()); material_xml->InsertEndChild(texture); - XMLElement* color = doc->NewElement("color"); + tinyxml2::XMLElement* color = doc->NewElement("color"); color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color).c_str()); material_xml->InsertEndChild(color); xml->InsertEndChild(material_xml); return true; } -bool exportSphere(Sphere &s, XMLElement *xml) +bool exportSphere(Sphere &s, tinyxml2::XMLElement *xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); // e.g. add - XMLElement *sphere_xml = doc->NewElement("sphere"); + tinyxml2::XMLElement *sphere_xml = doc->NewElement("sphere"); sphere_xml->SetAttribute("radius", urdf_export_helpers::values2str(s.radius).c_str()); xml->InsertEndChild(sphere_xml); return true; } -bool exportBox(Box &b, XMLElement *xml) +bool exportBox(Box &b, tinyxml2::XMLElement *xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); // e.g. add - XMLElement *box_xml = doc->NewElement("box"); + tinyxml2::XMLElement *box_xml = doc->NewElement("box"); box_xml->SetAttribute("size", urdf_export_helpers::values2str(b.dim).c_str()); xml->InsertEndChild(box_xml); return true; } -bool exportCylinder(Cylinder &y, XMLElement *xml) +bool exportCylinder(Cylinder &y, tinyxml2::XMLElement *xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); // e.g. add - XMLElement *cylinder_xml = doc->NewElement("cylinder"); + tinyxml2::XMLElement *cylinder_xml = doc->NewElement("cylinder"); cylinder_xml->SetAttribute("radius", urdf_export_helpers::values2str(y.radius).c_str()); cylinder_xml->SetAttribute("length", urdf_export_helpers::values2str(y.length).c_str()); xml->InsertEndChild(cylinder_xml); return true; } -bool exportMesh(Mesh &m, XMLElement *xml) +bool exportMesh(Mesh &m, tinyxml2::XMLElement *xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); // e.g. add - XMLElement *mesh_xml = doc->NewElement("mesh"); + tinyxml2::XMLElement *mesh_xml = doc->NewElement("mesh"); if (!m.filename.empty()) mesh_xml->SetAttribute("filename", m.filename.c_str()); mesh_xml->SetAttribute("scale", urdf_export_helpers::values2str(m.scale).c_str()); @@ -559,11 +557,11 @@ bool exportMesh(Mesh &m, XMLElement *xml) return true; } -bool exportGeometry(GeometrySharedPtr &geom, XMLElement *xml) +bool exportGeometry(GeometrySharedPtr &geom, tinyxml2::XMLElement *xml) { - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); - XMLElement *geometry_xml = doc->NewElement("geometry"); + tinyxml2::XMLElement *geometry_xml = doc->NewElement("geometry"); if (urdf::dynamic_pointer_cast(geom)) { exportSphere((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); @@ -593,24 +591,24 @@ bool exportGeometry(GeometrySharedPtr &geom, XMLElement *xml) return true; } -bool exportInertial(Inertial &i, XMLElement *xml) +bool exportInertial(Inertial &i, tinyxml2::XMLElement *xml) { // adds // // // // - XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLDocument *doc = xml->GetDocument(); - XMLElement *inertial_xml = doc->NewElement("inertial"); + tinyxml2::XMLElement *inertial_xml = doc->NewElement("inertial"); - XMLElement *mass_xml = doc->NewElement("mass"); + tinyxml2::XMLElement *mass_xml = doc->NewElement("mass"); mass_xml->SetAttribute("value", urdf_export_helpers::values2str(i.mass).c_str()); inertial_xml->InsertEndChild(mass_xml); exportPose(i.origin, inertial_xml); - XMLElement *inertia_xml = doc->NewElement("inertia"); + tinyxml2::XMLElement *inertia_xml = doc->NewElement("inertia"); inertia_xml->SetAttribute("ixx", urdf_export_helpers::values2str(i.ixx).c_str()); inertia_xml->SetAttribute("ixy", urdf_export_helpers::values2str(i.ixy).c_str()); inertia_xml->SetAttribute("ixz", urdf_export_helpers::values2str(i.ixz).c_str()); @@ -624,7 +622,7 @@ bool exportInertial(Inertial &i, XMLElement *xml) return true; } -bool exportVisual(Visual &vis, XMLElement *xml) +bool exportVisual(Visual &vis, tinyxml2::XMLElement *xml) { // // @@ -633,9 +631,9 @@ bool exportVisual(Visual &vis, XMLElement *xml) // // // - XMLDocument * doc = xml->GetDocument(); + tinyxml2::XMLDocument * doc = xml->GetDocument(); - XMLElement * visual_xml = doc->NewElement("visual"); + tinyxml2::XMLElement * visual_xml = doc->NewElement("visual"); exportPose(vis.origin, visual_xml); @@ -649,7 +647,7 @@ bool exportVisual(Visual &vis, XMLElement *xml) return true; } -bool exportCollision(Collision &col, XMLElement* xml) +bool exportCollision(Collision &col, tinyxml2::XMLElement* xml) { // // @@ -658,9 +656,9 @@ bool exportCollision(Collision &col, XMLElement* xml) // // // - XMLDocument * doc = xml->GetDocument(); + tinyxml2::XMLDocument * doc = xml->GetDocument(); - XMLElement * collision_xml = doc->NewElement("collision"); + tinyxml2::XMLElement * collision_xml = doc->NewElement("collision"); exportPose(col.origin, collision_xml); @@ -671,11 +669,11 @@ bool exportCollision(Collision &col, XMLElement* xml) return true; } -bool exportLink(Link &link, XMLElement* xml) +bool exportLink(Link &link, tinyxml2::XMLElement* xml) { - XMLDocument * doc = xml->GetDocument(); + tinyxml2::XMLDocument * doc = xml->GetDocument(); - XMLElement * link_xml = doc->NewElement("link"); + tinyxml2::XMLElement * link_xml = doc->NewElement("link"); link_xml->SetAttribute("name", link.name.c_str()); if (link.inertial) diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 9abfc174..5e2b6192 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -42,13 +42,11 @@ #include #include -using namespace tinyxml2; - namespace urdf{ -bool parseMaterial(Material &material, XMLElement *config, bool only_name_is_ok); -bool parseLink(Link &link, XMLElement *config); -bool parseJoint(Joint &joint, XMLElement *config); +bool parseMaterial(Material &material, tinyxml2::XMLElement *config, bool only_name_is_ok); +bool parseLink(Link &link, tinyxml2::XMLElement *config); +bool parseJoint(Joint &joint, tinyxml2::XMLElement *config); ModelInterfaceSharedPtr parseURDFFile(const std::string &path) { @@ -96,7 +94,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) ModelInterfaceSharedPtr model(new ModelInterface); model->clear(); - XMLDocument xml_doc; + tinyxml2::XMLDocument xml_doc; xml_doc.Parse(xml_string.c_str()); if (xml_doc.Error()) { @@ -106,7 +104,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) return model; } - XMLElement *robot_xml = xml_doc.FirstChildElement("robot"); + tinyxml2::XMLElement *robot_xml = xml_doc.FirstChildElement("robot"); if (!robot_xml) { CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the xml file"); @@ -140,7 +138,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) } // Get all Material elements - for (XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) + for (tinyxml2::XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) { MaterialSharedPtr material; material.reset(new Material); @@ -169,7 +167,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) } // Get all Link elements - for (XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) + for (tinyxml2::XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) { LinkSharedPtr link; link.reset(new Link); @@ -212,7 +210,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) } // Get all Joint elements - for (XMLElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) + for (tinyxml2::XMLElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) { JointSharedPtr joint; joint.reset(new Joint); @@ -272,14 +270,14 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) return model; } -bool exportMaterial(Material &material, XMLElement *config); -bool exportLink(Link &link, XMLElement *config); -bool exportJoint(Joint &joint, XMLElement *config); +bool exportMaterial(Material &material, tinyxml2::XMLElement *config); +bool exportLink(Link &link, tinyxml2::XMLElement *config); +bool exportJoint(Joint &joint, tinyxml2::XMLElement *config); void exportURDF(const ModelInterface &model, std::string &xml_string) { - XMLDocument doc; - XMLElement *robot = doc.NewElement("robot"); + tinyxml2::XMLDocument doc; + tinyxml2::XMLElement *robot = doc.NewElement("robot"); robot->SetAttribute("name", model.name_.c_str()); doc.InsertEndChild(robot); @@ -302,7 +300,7 @@ void exportURDF(const ModelInterface &model, std::string &xml_string) exportJoint(*(j->second), robot); } - XMLPrinter printer; + tinyxml2::XMLPrinter printer; doc.Print( &printer ); xml_string = std::string(printer.CStr()); diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp index 4a07dadc..fba0220e 100644 --- a/urdf_parser/src/pose.cpp +++ b/urdf_parser/src/pose.cpp @@ -46,8 +46,6 @@ #include #include -using namespace tinyxml2; - namespace urdf_export_helpers { std::string values2str(unsigned int count, const double *values, double (*conv)(double)) @@ -92,7 +90,7 @@ std::string values2str(double d) namespace urdf{ -bool parsePose(Pose &pose, XMLElement* xml) +bool parsePose(Pose &pose, tinyxml2::XMLElement* xml) { pose.clear(); if (xml) @@ -124,10 +122,10 @@ bool parsePose(Pose &pose, XMLElement* xml) return true; } -bool exportPose(Pose &pose, XMLElement* xml) +bool exportPose(Pose &pose, tinyxml2::XMLElement* xml) { - XMLDocument *doc = xml->GetDocument(); - XMLElement *origin = doc->NewElement("origin"); + tinyxml2::XMLDocument *doc = xml->GetDocument(); + tinyxml2::XMLElement *origin = doc->NewElement("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.c_str()); diff --git a/urdf_parser/src/twist.cpp b/urdf_parser/src/twist.cpp index 98ada76e..bab7cdf3 100644 --- a/urdf_parser/src/twist.cpp +++ b/urdf_parser/src/twist.cpp @@ -45,11 +45,9 @@ #include #include -using namespace tinyxml2; - namespace urdf{ -bool parseTwist(Twist &twist, XMLElement* xml) +bool parseTwist(Twist &twist, tinyxml2::XMLElement* xml) { twist.clear(); if (xml) diff --git a/urdf_parser/src/urdf_model_state.cpp b/urdf_parser/src/urdf_model_state.cpp index a7d2ad69..8dc60dba 100644 --- a/urdf_parser/src/urdf_model_state.cpp +++ b/urdf_parser/src/urdf_model_state.cpp @@ -50,11 +50,9 @@ #include #include -using namespace tinyxml2; - namespace urdf{ -bool parseModelState(ModelState &ms, XMLElement* config) +bool parseModelState(ModelState &ms, tinyxml2::XMLElement* config) { ms.clear(); @@ -77,7 +75,7 @@ bool parseModelState(ModelState &ms, XMLElement* config) } } - XMLElement *joint_state_elem = config->FirstChildElement("joint_state"); + tinyxml2::XMLElement *joint_state_elem = config->FirstChildElement("joint_state"); if (joint_state_elem) { JointStateSharedPtr joint_state; diff --git a/urdf_parser/src/urdf_sensor.cpp b/urdf_parser/src/urdf_sensor.cpp index 60cb6da0..7971d982 100644 --- a/urdf_parser/src/urdf_sensor.cpp +++ b/urdf_parser/src/urdf_sensor.cpp @@ -49,18 +49,16 @@ #include #include -using namespace tinyxml2; - namespace urdf{ -bool parsePose(Pose &pose, XMLElement* xml); +bool parsePose(Pose &pose, tinyxml2::XMLElement* xml); -bool parseCamera(Camera &camera, XMLElement* config) +bool parseCamera(Camera &camera, tinyxml2::XMLElement* config) { camera.clear(); camera.type = VisualSensor::CAMERA; - XMLElement *image = config->FirstChildElement("image"); + tinyxml2::XMLElement *image = config->FirstChildElement("image"); if (image) { const char* width_char = image->Attribute("width"); @@ -177,12 +175,12 @@ bool parseCamera(Camera &camera, XMLElement* config) return true; } -bool parseRay(Ray &ray, XMLElement* config) +bool parseRay(Ray &ray, tinyxml2::XMLElement* config) { ray.clear(); ray.type = VisualSensor::RAY; - XMLElement *horizontal = config->FirstChildElement("horizontal"); + tinyxml2::XMLElement *horizontal = config->FirstChildElement("horizontal"); if (horizontal) { const char* samples_char = horizontal->Attribute("samples"); @@ -238,7 +236,7 @@ bool parseRay(Ray &ray, XMLElement* config) } } - XMLElement *vertical = config->FirstChildElement("vertical"); + tinyxml2::XMLElement *vertical = config->FirstChildElement("vertical"); if (vertical) { const char* samples_char = vertical->Attribute("samples"); @@ -296,12 +294,12 @@ bool parseRay(Ray &ray, XMLElement* config) return false; } -VisualSensorSharedPtr parseVisualSensor(XMLElement *g) +VisualSensorSharedPtr parseVisualSensor(tinyxml2::XMLElement *g) { VisualSensorSharedPtr visual_sensor; // get sensor type - XMLElement *sensor_xml; + tinyxml2::XMLElement *sensor_xml; if (g->FirstChildElement("camera")) { Camera *camera = new Camera(); @@ -326,7 +324,7 @@ VisualSensorSharedPtr parseVisualSensor(XMLElement *g) } -bool parseSensor(Sensor &sensor, XMLElement* config) +bool parseSensor(Sensor &sensor, tinyxml2::XMLElement* config) { sensor.clear(); @@ -348,7 +346,7 @@ bool parseSensor(Sensor &sensor, XMLElement* config) sensor.parent_link_name = std::string(parent_link_name_char); // parse origin - XMLElement *o = config->FirstChildElement("origin"); + tinyxml2::XMLElement *o = config->FirstChildElement("origin"); if (o) { if (!parsePose(sensor.origin, o)) diff --git a/urdf_parser/src/world.cpp b/urdf_parser/src/world.cpp index f20f768a..210a63d3 100644 --- a/urdf_parser/src/world.cpp +++ b/urdf_parser/src/world.cpp @@ -48,21 +48,19 @@ #endif #include -using namespace tinyxml2; - namespace urdf{ -bool parseWorld(World &/*world*/, XMLElement* /*config*/) +bool parseWorld(World &/*world*/, tinyxml2::XMLElement* /*config*/) { // to be implemented return true; } -bool exportWorld(World &world, XMLElement* xml) +bool exportWorld(World &world, tinyxml2::XMLElement* xml) { - XMLDocument * doc = xml->GetDocument(); - XMLElement * world_xml = doc->NewElement("world"); + tinyxml2::XMLDocument * doc = xml->GetDocument(); + tinyxml2::XMLElement * world_xml = doc->NewElement("world"); world_xml->SetAttribute("name", world.name.c_str()); // to be implemented