Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Handle Octomap #127

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Handle Octomap #127

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4498622
Add octomap
mpowelson May 20, 2019
186c172
Add parse_octomap unit test
mpowelson May 21, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
78 changes: 78 additions & 0 deletions urdf_parser/src/link.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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");
Expand All @@ -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!");
Expand Down
64 changes: 64 additions & 0 deletions urdf_parser/test/urdf_unit_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down