From 0bca4966ee00b8f097af0b945b3250a0f4fed4ca 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 | 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 }