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)
{