Add multisphere

urdf_parser/src/link.cpp

@@ -216,6 +216,35 @@ bool parseMesh(Mesh &m, TiXmlElement *c)
   return true;
 }
 
+bool parseMultiSphere(MultiSphere &m, TiXmlElement *c)
+{
+  m.clear();
+
+  m.type = Geometry::MULTISPHERE;
+  if (!c->Attribute("filename")) {
+    CONSOLE_BRIDGE_logError("Multi-sphere 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("Multi-sphere 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 == "multisphere")
+  {
+    MultiSphere *m = new MultiSphere();
+    geom.reset(m);
+    if (parseMultiSphere(*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 exportMultiSphere(MultiSphere &m, TiXmlElement *xml)
+{
+  // e.g. add <multisphere filename="my_file" scale="1 1 1"/>
+  TiXmlElement *multisphere_xml = new TiXmlElement("multisphere");
+  if (!m.filename.empty())
+    multisphere_xml->SetAttribute("filename", m.filename);
+  multisphere_xml->SetAttribute("scale", urdf_export_helpers::values2str(m.scale));
+  xml->LinkEndChild(multisphere_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<MultiSphere>(geom))
+  {
+    exportMultiSphere((*(urdf::dynamic_pointer_cast<MultiSphere>(geom).get())), geometry_xml);
+  }
   else
   {
     CONSOLE_BRIDGE_logError("geometry not specified, I'll make one up for you!");