21 #ifdef HAS_SYSTEM_OCTOMAP
22 #include <octomap/OcTree.h>
23 #include <octomap/octomap.h>
43 map.updateVoxel(1, 1, 1,
true);
45 map.updateVoxel(1.5, 1, 1,
true);
46 map.updateVoxel(1.5, 1, 1,
true);
47 map.updateVoxel(1.5, 1, 1,
true);
49 map.updateVoxel(-1, -1, 1,
false);
56 is_mapped = map.getPointOccupancy(pt.
x, pt.
y, pt.
z, occup);
57 cout <<
"pt: " << pt <<
" is mapped?: " << (is_mapped ?
"YES" :
"NO")
58 <<
" occupancy: " << occup << endl;
61 is_mapped = map.getPointOccupancy(pt.x, pt.y, pt.z, occup);
62 cout <<
"pt: " << pt <<
" is mapped?: " << (is_mapped ?
"YES" :
"NO")
63 <<
" occupancy: " << occup << endl;
70 map.insertObservation(scan1);
85 scene->insert(gl_grid);
88 map.renderingOptions.generateGridLines =
true;
89 map.getAsOctoMapVoxels(*gl_map);
91 gl_map->showGridLines(
false);
94 scene->insert(gl_map);
96 win.unlockAccess3DScene();
100 #ifdef HAS_SYSTEM_OCTOMAP
102 const auto& om = map.getOctomap<octomap::OcTree>();
103 for (
auto it = om.begin_leafs(); it != om.end_leafs(); ++it)
105 const octomap::point3d pt = it.getCoordinate();
106 cout <<
"pt: " << pt <<
" -> occupancy = " << it->getOccupancy()
112 cout <<
"Close the window to exit" << endl;
114 bool update_msg =
true;
120 const unsigned int k =
win.getPushedKey();
126 gl_map->showGridLines(!gl_map->areGridLinesVisible());
142 gl_map->enableLights(!gl_map->areLightsEnabled());
155 "Commands: 'g' (grids=%s) | 'f' (freespace=%s) | 'o' "
156 "(occupied=%s) | 'l' (lights=%s)",
157 gl_map->areGridLinesVisible() ?
"YES" :
"NO",
161 gl_map->areLightsEnabled() ?
"YES" :
"NO"));
166 std::this_thread::sleep_for(10ms);
179 cout <<
"MRPT exception caught: " << e.what() << endl;
184 printf(
"Another exception!!");