Handle SDF

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 <sdf filename="my_file" scale="1 1 1"/>
+  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<Mesh>(geom).get())), geometry_xml);
   }
+  else if (urdf::dynamic_pointer_cast<SDF>(geom))
+  {
+    exportSDF((*(urdf::dynamic_pointer_cast<SDF>(geom).get())), geometry_xml);
+  }
   else
   {
     CONSOLE_BRIDGE_logError("geometry not specified, I'll make one up for you!");

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 =
+    "<robot name=\"test\">"
+    "  <joint name=\"j1\" type=\"fixed\">"
+    "    <parent link=\"l1\"/>"
+    "    <child link=\"l2\"/>"
+    "  </joint>"
+    "  <link name=\"l1\">"
+    "    <visual>"
+    "      <geometry>"
+    "        <sdf filename=\"sdf.filetype\" scale=\"1.1 2.2 3.3\"/>"
+    "      </geometry>"
+    "    </visual>"
+    "  </link>"
+    "  <link name=\"l2\">"
+    "    <collision>"
+    "      <geometry>"
+    "        <sdf filename=\"sdf.filetype\" />"
+    "      </geometry>"
+    "    </collision>"
+    "  </link>"
+    "</robot>";
+
+  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<urdf::SDF> c = std::dynamic_pointer_cast<urdf::SDF>(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<urdf::SDF> c2 = std::dynamic_pointer_cast<urdf::SDF>(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)
 {