From 44986224599123252644641cf5b11d3a6f8095c6 Mon Sep 17 00:00:00 2001 From: Matthew Powelson Date: Mon, 20 May 2019 15:49:11 -0500 Subject: [PATCH 1/2] Add octomap --- urdf_parser/src/link.cpp | 78 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index d3a7b2f2..163061f5 100644 --- a/urdf_parser/src/link.cpp +++ b/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 + 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(geom).get())), geometry_xml); } + else if (urdf::dynamic_pointer_cast(geom)) + { + exportOctomap((*(urdf::dynamic_pointer_cast(geom).get())), geometry_xml); + } else { CONSOLE_BRIDGE_logError("geometry not specified, I'll make one up for you!"); From 186c172836a48aa1a3949c178daab17b7797dc16 Mon Sep 17 00:00:00 2001 From: Matthew Powelson Date: Tue, 21 May 2019 09:03:17 -0500 Subject: [PATCH 2/2] Add parse_octomap unit test --- urdf_parser/test/urdf_unit_test.cpp | 64 +++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index c46dd326..102e5d80 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/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 = + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + ""; + + 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 c = std::dynamic_pointer_cast(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 c2 = std::dynamic_pointer_cast(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 c3 = std::dynamic_pointer_cast(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) {