diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index 858b236c..b7419483 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -51,8 +51,6 @@ #include "exportdecl.h" -using namespace tinyxml2; - namespace urdf_export_helpers { URDFDOM_DLLAPI std::string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL); @@ -145,13 +143,13 @@ namespace urdf{ URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string); URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string &path); - URDFDOM_DLLAPI XMLDocument* exportURDF(ModelInterfaceSharedPtr &model); - URDFDOM_DLLAPI XMLDocument* exportURDF(const ModelInterface &model); - URDFDOM_DLLAPI bool parsePose(Pose&, XMLElement*); - URDFDOM_DLLAPI bool parseCamera(Camera&, XMLElement*); - URDFDOM_DLLAPI bool parseRay(Ray&, XMLElement*); - URDFDOM_DLLAPI bool parseSensor(Sensor&, XMLElement*); - URDFDOM_DLLAPI bool parseModelState(ModelState&, XMLElement*); + URDFDOM_DLLAPI tinyxml2::XMLDocument* exportURDF(ModelInterfaceSharedPtr &model); + URDFDOM_DLLAPI tinyxml2::XMLDocument* 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 05140ee9..670e5912 100644 --- a/urdf_parser/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software Ligcense Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -45,9 +45,9 @@ 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(); @@ -94,7 +94,7 @@ bool parseJointDynamics(JointDynamics &jd, XMLElement* config) } } -bool parseJointLimits(JointLimits &jl, XMLElement* config) +bool parseJointLimits(JointLimits &jl, tinyxml2::XMLElement* config) { jl.clear(); @@ -165,7 +165,7 @@ bool parseJointLimits(JointLimits &jl, XMLElement* config) return true; } -bool parseJointSafety(JointSafety &js, XMLElement* config) +bool parseJointSafety(JointSafety &js, tinyxml2::XMLElement* config) { js.clear(); @@ -239,7 +239,7 @@ bool parseJointSafety(JointSafety &js, XMLElement* config) return true; } -bool parseJointCalibration(JointCalibration &jc, XMLElement* config) +bool parseJointCalibration(JointCalibration &jc, tinyxml2::XMLElement* config) { jc.clear(); @@ -280,7 +280,7 @@ bool parseJointCalibration(JointCalibration &jc, XMLElement* config) return true; } -bool parseJointMimic(JointMimic &jm, XMLElement* config) +bool parseJointMimic(JointMimic &jm, tinyxml2::XMLElement* config) { jm.clear(); @@ -294,14 +294,14 @@ bool parseJointMimic(JointMimic &jm, XMLElement* config) } 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; + jm.multiplier = 1; } else { @@ -313,7 +313,7 @@ bool parseJointMimic(JointMimic &jm, XMLElement* config) } } - + // Get mimic offset const char* offset_str = config->Attribute("offset"); if (offset_str == NULL) @@ -334,7 +334,7 @@ bool parseJointMimic(JointMimic &jm, XMLElement* config) return true; } -bool parseJoint(Joint &joint, XMLElement* config) +bool parseJoint(Joint &joint, tinyxml2::XMLElement* config) { joint.clear(); @@ -348,7 +348,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()); @@ -365,7 +365,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"); @@ -380,7 +380,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"); @@ -401,7 +401,7 @@ bool parseJoint(Joint &joint, XMLElement* config) 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; @@ -425,7 +425,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); @@ -445,7 +445,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()); @@ -463,12 +463,12 @@ bool parseJoint(Joint &joint, XMLElement* config) } else if (joint.type == Joint::PRISMATIC) { - CONSOLE_BRIDGE_logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str()); + CONSOLE_BRIDGE_logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str()); return false; } // 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()); @@ -481,7 +481,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()); @@ -494,7 +494,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()); @@ -507,7 +507,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()); @@ -524,20 +524,20 @@ 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) { - XMLElement *dynamics_xml = xml->InsertNewChildElement("dynamics"); + tinyxml2::XMLElement *dynamics_xml = xml->InsertNewChildElement("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->LinkEndChild(dynamics_xml); return true; } -bool exportJointLimits(JointLimits &jl, XMLElement* xml) +bool exportJointLimits(JointLimits &jl, tinyxml2::XMLElement* xml) { - XMLElement *limit_xml = xml->InsertNewChildElement("limit"); + tinyxml2::XMLElement *limit_xml = xml->InsertNewChildElement("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()); @@ -546,9 +546,9 @@ bool exportJointLimits(JointLimits &jl, XMLElement* xml) return true; } -bool exportJointSafety(JointSafety &js, XMLElement* xml) +bool exportJointSafety(JointSafety &js, tinyxml2::XMLElement* xml) { - XMLElement *safety_xml = xml->InsertNewChildElement("safety_controller"); + tinyxml2::XMLElement *safety_xml = xml->InsertNewChildElement("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()); @@ -557,11 +557,11 @@ bool exportJointSafety(JointSafety &js, XMLElement* xml) return true; } -bool exportJointCalibration(JointCalibration &jc, XMLElement* xml) +bool exportJointCalibration(JointCalibration &jc, tinyxml2::XMLElement* xml) { if (jc.falling || jc.rising) { - XMLElement *calibration_xml = xml->InsertNewChildElement("calibration"); + tinyxml2::XMLElement *calibration_xml = xml->InsertNewChildElement("calibration"); if (jc.falling) calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling).c_str()); if (jc.rising) @@ -572,11 +572,11 @@ bool exportJointCalibration(JointCalibration &jc, XMLElement* xml) return true; } -bool exportJointMimic(JointMimic &jm, XMLElement* xml) +bool exportJointMimic(JointMimic &jm, tinyxml2::XMLElement* xml) { if (!jm.joint_name.empty()) { - XMLElement *mimic_xml = xml->InsertNewChildElement("mimic"); + tinyxml2::XMLElement *mimic_xml = xml->InsertNewChildElement("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()); @@ -585,9 +585,9 @@ bool exportJointMimic(JointMimic &jm, XMLElement* xml) return true; } -bool exportJoint(Joint &joint, XMLElement* xml) +bool exportJoint(Joint &joint, tinyxml2::XMLElement* xml) { - XMLElement * joint_xml = xml->InsertNewChildElement("joint"); + tinyxml2::XMLElement * joint_xml = xml->InsertNewChildElement("joint"); joint_xml->SetAttribute("name", joint.name.c_str()); if (joint.type == urdf::Joint::PLANAR) joint_xml->SetAttribute("type", "planar"); @@ -608,17 +608,17 @@ bool exportJoint(Joint &joint, XMLElement* xml) exportPose(joint.parent_to_joint_origin_transform, joint_xml); // axis - XMLElement * axis_xml = joint_xml->InsertNewChildElement("axis"); + tinyxml2::XMLElement * axis_xml = joint_xml->InsertNewChildElement("axis"); axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(joint.axis).c_str()); joint_xml->LinkEndChild(axis_xml); - // parent - XMLElement * parent_xml = joint_xml->InsertNewChildElement("parent"); + // parent + tinyxml2::XMLElement * parent_xml = joint_xml->InsertNewChildElement("parent"); parent_xml->SetAttribute("link", joint.parent_link_name.c_str()); joint_xml->LinkEndChild(parent_xml); // child - XMLElement * child_xml = joint_xml->InsertNewChildElement("child"); + tinyxml2::XMLElement * child_xml = joint_xml->InsertNewChildElement("child"); child_xml->SetAttribute("link", joint.child_link_name.c_str()); joint_xml->LinkEndChild(child_xml); diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index 12a8de96..49b16121 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -50,9 +50,9 @@ 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; @@ -64,11 +64,11 @@ bool parseMaterial(Material &material, XMLElement *config, bool only_name_is_ok) CONSOLE_BRIDGE_logError("Material must contain a name attribute"); return false; } - + material.name = config->Attribute("name"); // texture - XMLElement *t = config->FirstChildElement("texture"); + tinyxml2::XMLElement *t = config->FirstChildElement("texture"); if (t) { if (t->Attribute("filename")) @@ -79,7 +79,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")) { @@ -88,7 +88,7 @@ bool parseMaterial(Material &material, XMLElement *config, bool only_name_is_ok) material.color.init(c->Attribute("rgba")); has_rgb = true; } - catch (ParseError &e) { + catch (ParseError &e) { material.color.clear(); CONSOLE_BRIDGE_logError(std::string("Material [" + material.name + "] has malformed color rgba values: " + e.what()).c_str()); } @@ -107,7 +107,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(); @@ -130,10 +130,10 @@ bool parseSphere(Sphere &s, XMLElement *c) return true; } -bool parseBox(Box &b, XMLElement *c) +bool parseBox(Box &b, tinyxml2::XMLElement *c) { b.clear(); - + b.type = Geometry::BOX; if (!c->Attribute("size")) { @@ -153,7 +153,7 @@ bool parseBox(Box &b, XMLElement *c) return true; } -bool parseCylinder(Cylinder &y, XMLElement *c) +bool parseCylinder(Cylinder &y, tinyxml2::XMLElement *c) { y.clear(); @@ -187,7 +187,7 @@ bool parseCylinder(Cylinder &y, XMLElement *c) } -bool parseMesh(Mesh &m, XMLElement *c) +bool parseMesh(Mesh &m, tinyxml2::XMLElement *c) { m.clear(); @@ -216,12 +216,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."); @@ -255,30 +255,30 @@ GeometrySharedPtr parseGeometry(XMLElement *g) Mesh *m = new Mesh(); geom.reset(m); if (parseMesh(*m, shape)) - return geom; + return geom; } else { CONSOLE_BRIDGE_logError("Unknown geometry type '%s'", type_name.c_str()); return geom; } - + 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"); @@ -300,7 +300,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"); @@ -346,19 +346,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; @@ -368,7 +368,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")) { @@ -376,7 +376,7 @@ bool parseVisual(Visual &vis, XMLElement *config) 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)) @@ -384,23 +384,23 @@ bool parseVisual(Visual &vis, XMLElement *config) vis.material.reset(); } } - + 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; @@ -412,9 +412,9 @@ bool parseCollision(Collision &col, XMLElement* config) return true; } -bool parseLink(Link &link, XMLElement* config) +bool parseLink(Link &link, tinyxml2::XMLElement* config) { - + link.clear(); const char *name_char = config->Attribute("name"); @@ -426,7 +426,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()); @@ -438,7 +438,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; @@ -459,14 +459,14 @@ bool parseLink(Link &link, XMLElement* config) // 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 (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()); if (parseCollision(*col, col_xml)) - { + { link.collision_array.push_back(col); } else @@ -476,8 +476,8 @@ bool parseLink(Link &link, XMLElement* config) return false; } } - - // Collision (optional) + + // Collision (optional) // Assign the first collision to the .collision ptr, if it exists if (!link.collision_array.empty()) link.collision = link.collision_array[0]; @@ -486,57 +486,57 @@ 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) { - XMLElement* material_xml = xml->InsertNewChildElement("material"); + tinyxml2::XMLElement* material_xml = xml->InsertNewChildElement("material"); material_xml->SetAttribute("name", material.name.c_str()); - XMLElement* texture = material_xml->InsertNewChildElement("texture"); + tinyxml2::XMLElement* texture = material_xml->InsertNewChildElement("texture"); if (!material.texture_filename.empty()) texture->SetAttribute("filename", material.texture_filename.c_str()); material_xml->LinkEndChild(texture); - XMLElement* color = material_xml->InsertNewChildElement("color"); + tinyxml2::XMLElement* color = material_xml->InsertNewChildElement("color"); color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color).c_str()); material_xml->LinkEndChild(color); xml->LinkEndChild(material_xml); return true; } -bool exportSphere(Sphere &s, XMLElement *xml) +bool exportSphere(Sphere &s, tinyxml2::XMLElement *xml) { // e.g. add - XMLElement *sphere_xml = xml->InsertNewChildElement("sphere"); + tinyxml2::XMLElement *sphere_xml = xml->InsertNewChildElement("sphere"); sphere_xml->SetAttribute("radius", urdf_export_helpers::values2str(s.radius).c_str()); xml->LinkEndChild(sphere_xml); return true; } -bool exportBox(Box &b, XMLElement *xml) +bool exportBox(Box &b, tinyxml2::XMLElement *xml) { // e.g. add - XMLElement *box_xml = xml->InsertNewChildElement("box"); + tinyxml2::XMLElement *box_xml = xml->InsertNewChildElement("box"); box_xml->SetAttribute("size", urdf_export_helpers::values2str(b.dim).c_str()); xml->LinkEndChild(box_xml); return true; } -bool exportCylinder(Cylinder &y, XMLElement *xml) +bool exportCylinder(Cylinder &y, tinyxml2::XMLElement *xml) { // e.g. add - XMLElement *cylinder_xml = xml->InsertNewChildElement("cylinder"); + tinyxml2::XMLElement *cylinder_xml = xml->InsertNewChildElement("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->LinkEndChild(cylinder_xml); return true; } -bool exportMesh(Mesh &m, XMLElement *xml) +bool exportMesh(Mesh &m, tinyxml2::XMLElement *xml) { // e.g. add - XMLElement *mesh_xml = xml->InsertNewChildElement("mesh"); + tinyxml2::XMLElement *mesh_xml = xml->InsertNewChildElement("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()); @@ -544,9 +544,9 @@ bool exportMesh(Mesh &m, XMLElement *xml) return true; } -bool exportGeometry(GeometrySharedPtr &geom, XMLElement *xml) +bool exportGeometry(GeometrySharedPtr &geom, tinyxml2::XMLElement *xml) { - XMLElement *geometry_xml = xml->InsertNewChildElement("geometry"); + tinyxml2::XMLElement *geometry_xml = xml->InsertNewChildElement("geometry"); if (urdf::dynamic_pointer_cast(geom)) { exportSphere((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); @@ -576,22 +576,22 @@ bool exportGeometry(GeometrySharedPtr &geom, XMLElement *xml) return true; } -bool exportInertial(Inertial &i, XMLElement *xml) +bool exportInertial(Inertial &i, tinyxml2::XMLElement *xml) { // adds // // // // - XMLElement *inertial_xml = xml->InsertNewChildElement("inertial"); + tinyxml2::XMLElement *inertial_xml = xml->InsertNewChildElement("inertial"); - XMLElement *mass_xml = inertial_xml->InsertNewChildElement("mass"); + tinyxml2::XMLElement *mass_xml = inertial_xml->InsertNewChildElement("mass"); mass_xml->SetAttribute("value", urdf_export_helpers::values2str(i.mass).c_str()); inertial_xml->LinkEndChild(mass_xml); exportPose(i.origin, inertial_xml); - XMLElement *inertia_xml = inertial_xml->InsertNewChildElement("inertia"); + tinyxml2::XMLElement *inertia_xml = inertial_xml->InsertNewChildElement("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()); @@ -601,11 +601,11 @@ bool exportInertial(Inertial &i, XMLElement *xml) inertial_xml->LinkEndChild(inertia_xml); xml->LinkEndChild(inertial_xml); - + return true; } -bool exportVisual(Visual &vis, XMLElement *xml) +bool exportVisual(Visual &vis, tinyxml2::XMLElement *xml) { // // @@ -614,7 +614,7 @@ bool exportVisual(Visual &vis, XMLElement *xml) // // // - XMLElement * visual_xml = xml->InsertNewChildElement("visual"); + tinyxml2::XMLElement * visual_xml = xml->InsertNewChildElement("visual"); exportPose(vis.origin, visual_xml); @@ -628,8 +628,8 @@ bool exportVisual(Visual &vis, XMLElement *xml) return true; } -bool exportCollision(Collision &col, XMLElement* xml) -{ +bool exportCollision(Collision &col, tinyxml2::XMLElement* xml) +{ // // // @@ -637,7 +637,7 @@ bool exportCollision(Collision &col, XMLElement* xml) // // // - XMLElement * collision_xml = xml->InsertNewChildElement("collision"); + tinyxml2::XMLElement * collision_xml = xml->InsertNewChildElement("collision"); exportPose(col.origin, collision_xml); @@ -648,9 +648,9 @@ bool exportCollision(Collision &col, XMLElement* xml) return true; } -bool exportLink(Link &link, XMLElement* xml) +bool exportLink(Link &link, tinyxml2::XMLElement* xml) { - XMLElement * link_xml = xml->InsertNewChildElement("link"); + tinyxml2::XMLElement * link_xml = xml->InsertNewChildElement("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 fea8511f..e3592939 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -43,9 +43,9 @@ 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) { @@ -93,7 +93,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()) { @@ -103,7 +103,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"); @@ -137,7 +137,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); @@ -166,7 +166,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); @@ -209,7 +209,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); @@ -243,7 +243,7 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) parent_link_tree.clear(); // building tree: name mapping - try + try { model->initTree(parent_link_tree); } @@ -265,18 +265,18 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) model.reset(); return model; } - + return model; } -bool exportMaterial(Material &material, XMLElement *config); -bool exportLink(Link &link, XMLElement *config); -bool exportJoint(Joint &joint, XMLElement *config); -XMLDocument* exportURDF(const ModelInterface &model) +bool exportMaterial(Material &material, tinyxml2::XMLElement *config); +bool exportLink(Link &link, tinyxml2::XMLElement *config); +bool exportJoint(Joint &joint, tinyxml2::XMLElement *config); +tinyxml2::XMLDocument* exportURDF(const ModelInterface &model) { - XMLDocument *doc = new XMLDocument(); + tinyxml2::XMLDocument *doc = new tinyxml2::XMLDocument(); - XMLElement* robot = doc->NewElement("robot"); + tinyxml2::XMLElement* robot = doc->NewElement("robot"); robot->SetAttribute("name", model.name_.c_str()); doc->LinkEndChild(robot); @@ -287,13 +287,13 @@ XMLDocument* exportURDF(const ModelInterface &model) exportMaterial(*(m->second), robot); } - for (std::map::const_iterator l=model.links_.begin(); l!=model.links_.end(); ++l) + 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) + + 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); @@ -301,12 +301,11 @@ XMLDocument* exportURDF(const ModelInterface &model) return doc; } - -XMLDocument* exportURDF(ModelInterfaceSharedPtr &model) + +tinyxml2::XMLDocument* exportURDF(ModelInterfaceSharedPtr &model) { return exportURDF(*model); } } - diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp index 70f06462..9a39adc5 100644 --- a/urdf_parser/src/pose.cpp +++ b/urdf_parser/src/pose.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -87,7 +87,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) @@ -119,9 +119,9 @@ bool parsePose(Pose &pose, XMLElement* xml) return true; } -bool exportPose(Pose &pose, XMLElement* xml) +bool exportPose(Pose &pose, tinyxml2::XMLElement* xml) { - XMLElement* origin = xml->InsertNewChildElement("origin"); + tinyxml2::XMLElement* origin = xml->InsertNewChildElement("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()); @@ -131,5 +131,3 @@ bool exportPose(Pose &pose, XMLElement* xml) } } - - diff --git a/urdf_parser/src/urdf_model_state.cpp b/urdf_parser/src/urdf_model_state.cpp index 62473d62..7f117ad1 100644 --- a/urdf_parser/src/urdf_model_state.cpp +++ b/urdf_parser/src/urdf_model_state.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -49,7 +49,7 @@ #include namespace urdf{ -bool parseModelState(ModelState &ms, XMLElement* config) +bool parseModelState(ModelState &ms, tinyxml2::XMLElement* config) { ms.clear(); @@ -72,7 +72,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; @@ -86,7 +86,7 @@ bool parseModelState(ModelState &ms, XMLElement* config) 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) @@ -150,5 +150,3 @@ bool parseModelState(ModelState &ms, XMLElement* config) } - - diff --git a/urdf_parser/src/urdf_sensor.cpp b/urdf_parser/src/urdf_sensor.cpp index 8d2c6735..8332920c 100644 --- a/urdf_parser/src/urdf_sensor.cpp +++ b/urdf_parser/src/urdf_sensor.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -49,14 +49,14 @@ 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"); @@ -114,7 +114,7 @@ bool parseCamera(Camera &camera, XMLElement* config) { CONSOLE_BRIDGE_logError("Camera sensor needs an image format attribute"); return false; - } + } const char* hfov_char = image->Attribute("hfov"); if (hfov_char) @@ -163,7 +163,7 @@ bool parseCamera(Camera &camera, XMLElement* config) CONSOLE_BRIDGE_logError("Camera sensor needs an image far attribute"); return false; } - + } else { @@ -173,12 +173,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"); @@ -233,8 +233,8 @@ 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"); @@ -292,12 +292,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(); @@ -322,7 +322,7 @@ VisualSensorSharedPtr parseVisualSensor(XMLElement *g) } -bool parseSensor(Sensor &sensor, XMLElement* config) +bool parseSensor(Sensor &sensor, tinyxml2::XMLElement* config) { sensor.clear(); @@ -344,7 +344,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)) @@ -358,5 +358,3 @@ bool parseSensor(Sensor &sensor, XMLElement* config) } - - diff --git a/urdf_parser/src/world.cpp b/urdf_parser/src/world.cpp index d22021a7..2d930e63 100644 --- a/urdf_parser/src/world.cpp +++ b/urdf_parser/src/world.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -46,7 +46,7 @@ namespace urdf{ -bool parseWorld(World &/*world*/, XMLElement* /*config*/) +bool parseWorld(World &/*world*/, tinyxml2::XMLElement* /*config*/) { // to be implemented @@ -54,9 +54,9 @@ bool parseWorld(World &/*world*/, XMLElement* /*config*/) return true; } -bool exportWorld(World &world, XMLElement* xml) +bool exportWorld(World &world, tinyxml2::XMLElement* xml) { - XMLElement * world_xml = xml->InsertNewChildElement("world"); + tinyxml2::XMLElement * world_xml = xml->InsertNewChildElement("world"); world_xml->SetAttribute("name", world.name.c_str()); // to be implemented