From 1458ccda1ef6d0e14de04c70ad19f12400f342df Mon Sep 17 00:00:00 2001 From: Sean Molenaar Date: Wed, 8 Feb 2023 20:01:45 +0100 Subject: [PATCH] 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 | 56 +- urdf_parser/src/link.cpp | 620 ++++++++++++++++++ urdf_parser/src/model.cpp | 44 +- 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, 1238 insertions(+), 17 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..e54b1482 100644 --- a/urdf_parser/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -40,8 +40,11 @@ #include #include #include -#include #include +#ifdef HAVE_TINYXML +#include +#endif +#include using namespace tinyxml2; @@ -653,4 +656,55 @@ 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; + } +} +#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..53c454a4 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -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 }