Handle Octomap

urdf_parser/src/link.cpp

@@ -216,6 +216,54 @@ bool parseMesh(Mesh &m, TiXmlElement *c)
   return true;
 }
 
+bool parseOctomap(Octomap &m, TiXmlElement *c)
+{
+  m.clear();
+
+  m.type = Geometry::OCTOMAP;
+  if (!c->Attribute("filename")) {
+    CONSOLE_BRIDGE_logError("Octomap 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("Octomap scale was specified, but could not be parsed: %s", e.what());
+      return false;
+    }
+  }
+  else
+  {
+    m.scale.x = m.scale.y = m.scale.z = 1;
+  }
+
+  if (c->Attribute("type")) {
+    std::string type_str = c->Attribute("type");
+    if (type_str == "box")
+      m.octomap_type = Octomap::BOX;
+    else if (type_str == "inside_sphere")
+      m.octomap_type = Octomap::INSIDE_SPHERE;
+    else if (type_str == "outside_sphere")
+      m.octomap_type = Octomap::OUTSIDE_SPHERE;
+    else
+    {
+      CONSOLE_BRIDGE_logError("Octomap [%s] has no known type [%s]", m.filename.c_str(), type_str.c_str());
+      return false;
+    }
+  }
+  else
+  {
+    m.octomap_type = Octomap::BOX;
+  }
+  return true;
+}
+
 GeometrySharedPtr parseGeometry(TiXmlElement *g)
 {
   GeometrySharedPtr geom;
@@ -257,6 +305,13 @@ GeometrySharedPtr parseGeometry(TiXmlElement *g)
     if (parseMesh(*m, shape))
       return geom;    
   }
+  else if (type_name == "octomap")
+  {
+    Octomap *m = new Octomap();
+    geom.reset(m);
+    if (parseOctomap(*m, shape))
+      return geom;
+  }
   else
   {
     CONSOLE_BRIDGE_logError("Unknown geometry type '%s'", type_name.c_str());
@@ -544,6 +599,25 @@ bool exportMesh(Mesh &m, TiXmlElement *xml)
   return true;
 }
 
+bool exportOctomap(Octomap &m, TiXmlElement *xml)
+{
+  // e.g. add <octomap filename="my_file" scale="1 1 1" type="box"/>
+  TiXmlElement *octomap_xml = new TiXmlElement("octomap");
+  if (!m.filename.empty())
+    octomap_xml->SetAttribute("filename", m.filename);
+  octomap_xml->SetAttribute("scale", urdf_export_helpers::values2str(m.scale));
+  if (m.octomap_type == Octomap::BOX)
+    octomap_xml->SetAttribute("type", "box");
+  else if (m.octomap_type == Octomap::INSIDE_SPHERE)
+    octomap_xml->SetAttribute("type", "inside_sphere");
+  else if (m.octomap_type == Octomap::OUTSIDE_SPHERE)
+    octomap_xml->SetAttribute("type", "outside_sphere");
+  else
+    CONSOLE_BRIDGE_logError("Octomap [%s] is not a known type", m.filename.c_str());
+  xml->LinkEndChild(octomap_xml);
+  return true;
+}
+
 bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml)
 {
   TiXmlElement *geometry_xml = new TiXmlElement("geometry");
@@ -563,6 +637,10 @@ bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml)
   {
     exportMesh((*(urdf::dynamic_pointer_cast<Mesh>(geom).get())), geometry_xml);
   }
+  else if (urdf::dynamic_pointer_cast<Octomap>(geom))
+  {
+    exportOctomap((*(urdf::dynamic_pointer_cast<Octomap>(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,70 @@ TEST(URDF_UNIT_TEST, parse_color_doubles)
   EXPECT_EQ(0.908, urdf->links_["l1"]->inertial->izz);
 }
 
+TEST(URDF_UNIT_TEST, parse_octomap)
+{
+  std::string link_str =
+    "<robot name=\"test\">"
+    "  <joint name=\"j1\" type=\"fixed\">"
+    "    <parent link=\"l1\"/>"
+    "    <child link=\"l2\"/>"
+    "  </joint>"
+    "  <joint name=\"j2\" type=\"fixed\">"
+    "    <parent link=\"l2\"/>"
+    "    <child link=\"l3\"/>"
+    "  </joint>"
+    "  <link name=\"l1\">"
+    "    <visual>"
+    "      <geometry>"
+    "        <octomap filename=\"octomap.filetype\" scale=\"1.1 2.2 3.3\" type=\"box\"/>"
+    "      </geometry>"
+    "    </visual>"
+    "  </link>"
+    "  <link name=\"l2\">"
+    "    <collision>"
+    "      <geometry>"
+    "        <octomap filename=\"octomap.filetype\" type=\"inside_sphere\"/>"
+    "      </geometry>"
+    "    </collision>"
+    "  </link>"
+    "  <link name=\"l3\">"
+    "    <collision>"
+    "      <geometry>"
+    "        <octomap filename=\"octomap.filetype\" type=\"outside_sphere\"/>"
+    "      </geometry>"
+    "    </collision>"
+    "  </link>"
+    "</robot>";
+
+  urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(link_str);
+
+  EXPECT_EQ(3, urdf->links_.size());
+  EXPECT_EQ(2, urdf->joints_.size());
+
+  EXPECT_EQ(urdf::Geometry::OCTOMAP, urdf->links_["l1"]->visual->geometry->type);
+  std::shared_ptr<urdf::Octomap> c = std::dynamic_pointer_cast<urdf::Octomap>(urdf->links_["l1"]->visual->geometry);
+  EXPECT_EQ("octomap.filetype", c->filename);
+  EXPECT_EQ(urdf::Octomap::BOX, c->octomap_type);
+  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::OCTOMAP, urdf->links_["l2"]->collision->geometry->type);
+  std::shared_ptr<urdf::Octomap> c2 = std::dynamic_pointer_cast<urdf::Octomap>(urdf->links_["l2"]->collision->geometry);
+  EXPECT_EQ("octomap.filetype", c2->filename);
+  EXPECT_EQ(urdf::Octomap::INSIDE_SPHERE, c2->octomap_type);
+  EXPECT_FLOAT_EQ(1.0, c2->scale.x);
+  EXPECT_FLOAT_EQ(1.0, c2->scale.y);
+  EXPECT_FLOAT_EQ(1.0, c2->scale.z);
+
+  EXPECT_EQ(urdf::Geometry::OCTOMAP, urdf->links_["l3"]->collision->geometry->type);
+  std::shared_ptr<urdf::Octomap> c3 = std::dynamic_pointer_cast<urdf::Octomap>(urdf->links_["l3"]->collision->geometry);
+  EXPECT_EQ("octomap.filetype", c3->filename);
+  EXPECT_EQ(urdf::Octomap::OUTSIDE_SPHERE, c3->octomap_type);
+  EXPECT_FLOAT_EQ(1.0, c3->scale.x);
+  EXPECT_FLOAT_EQ(1.0, c3->scale.y);
+  EXPECT_FLOAT_EQ(1.0, c3->scale.z);
+}
 
 int main(int argc, char **argv)
 {