diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index d3a7b2f2..16a2e531 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -216,6 +216,35 @@ bool parseMesh(Mesh &m, TiXmlElement *c) return true; } +bool parseSDF(SDF &m, TiXmlElement *c) +{ + m.clear(); + + m.type = Geometry::SDF; + if (!c->Attribute("filename")) { + CONSOLE_BRIDGE_logError("Signed Distance Field (SDF) 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("Signed Distance Field (SDF) 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; @@ -257,6 +286,13 @@ GeometrySharedPtr parseGeometry(TiXmlElement *g) if (parseMesh(*m, shape)) return geom; } + else if (type_name == "sdf") + { + SDF *m = new SDF(); + geom.reset(m); + if (parseSDF(*m, shape)) + return geom; + } else { CONSOLE_BRIDGE_logError("Unknown geometry type '%s'", type_name.c_str()); @@ -544,6 +580,17 @@ bool exportMesh(Mesh &m, TiXmlElement *xml) return true; } +bool exportSDF(SDF &m, TiXmlElement *xml) +{ + // e.g. add + TiXmlElement *sdf_xml = new TiXmlElement("sdf"); + if (!m.filename.empty()) + sdf_xml->SetAttribute("filename", m.filename); + sdf_xml->SetAttribute("scale", urdf_export_helpers::values2str(m.scale)); + xml->LinkEndChild(sdf_xml); + return true; +} + bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml) { TiXmlElement *geometry_xml = new TiXmlElement("geometry"); @@ -563,6 +610,10 @@ bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml) { exportMesh((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); } + else if (urdf::dynamic_pointer_cast(geom)) + { + exportSDF((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); + } else { CONSOLE_BRIDGE_logError("geometry not specified, I'll make one up for you!"); diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index c46dd326..840e6613 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -332,6 +332,49 @@ TEST(URDF_UNIT_TEST, parse_color_doubles) EXPECT_EQ(0.908, urdf->links_["l1"]->inertial->izz); } +TEST(URDF_UNIT_TEST, parse_sdf) +{ + std::string link_str = + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + ""; + + urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(link_str); + + EXPECT_EQ(2, urdf->links_.size()); + EXPECT_EQ(1, urdf->joints_.size()); + + EXPECT_EQ(urdf::Geometry::SDF, urdf->links_["l1"]->visual->geometry->type); + std::shared_ptr c = std::dynamic_pointer_cast(urdf->links_["l1"]->visual->geometry); + EXPECT_EQ("sdf.filetype", c->filename); + EXPECT_FLOAT_EQ(1.1, c->scale.x); + EXPECT_FLOAT_EQ(2.2, c->scale.y); + EXPECT_FLOAT_EQ(3.3, c->scale.z); + + EXPECT_EQ(urdf::Geometry::SDF, urdf->links_["l2"]->collision->geometry->type); + std::shared_ptr c2 = std::dynamic_pointer_cast(urdf->links_["l2"]->collision->geometry); + EXPECT_EQ("sdf.filetype", c2->filename); + EXPECT_FLOAT_EQ(1.0, c2->scale.x); + EXPECT_FLOAT_EQ(1.0, c2->scale.y); + EXPECT_FLOAT_EQ(1.0, c2->scale.z); +} int main(int argc, char **argv) {