| 42 | |
| 43 | |
| 44 | int main( int argc, char *argv[] ) { |
| 45 | if(argc<=1){ |
| 46 | std::cout<<"usage: "<<argv[0]<<" <octoMap.bt>"<<std::endl; |
| 47 | exit(0); |
| 48 | } |
| 49 | |
| 50 | octomap::OcTree *tree = NULL; |
| 51 | tree = new octomap::OcTree(0.05); |
| 52 | |
| 53 | //read in octotree |
| 54 | tree->readBinary(argv[1]); |
| 55 | |
| 56 | std::cout<<"read in tree, "<<tree->getNumLeafNodes()<<" leaves "<<std::endl; |
| 57 | |
| 58 | double x,y,z; |
| 59 | tree->getMetricMin(x,y,z); |
| 60 | octomap::point3d min(x,y,z); |
| 61 | //std::cout<<"Metric min: "<<x<<","<<y<<","<<z<<std::endl; |
| 62 | tree->getMetricMax(x,y,z); |
| 63 | octomap::point3d max(x,y,z); |
| 64 | //std::cout<<"Metric max: "<<x<<","<<y<<","<<z<<std::endl; |
| 65 | |
| 66 | bool unknownAsOccupied = true; |
| 67 | unknownAsOccupied = false; |
| 68 | float maxDist = 1.0; |
| 69 | //- the first argument ist the max distance at which distance computations are clamped |
| 70 | //- the second argument is the octomap |
| 71 | //- arguments 3 and 4 can be used to restrict the distance map to a subarea |
| 72 | //- argument 5 defines whether unknown space is treated as occupied or free |
| 73 | //The constructor copies data but does not yet compute the distance map |
| 74 | DynamicEDTOctomap distmap(maxDist, tree, min, max, unknownAsOccupied); |
| 75 | |
| 76 | //This computes the distance map |
| 77 | distmap.update(); |
| 78 | |
| 79 | //This is how you can query the map |
| 80 | octomap::point3d p(5.0,5.0,0.6); |
| 81 | //As we don't know what the dimension of the loaded map are, we modify this point |
| 82 | p.x() = min.x() + 0.3 * (max.x() - min.x()); |
| 83 | p.y() = min.y() + 0.6 * (max.y() - min.y()); |
| 84 | p.z() = min.z() + 0.5 * (max.z() - min.z()); |
| 85 | |
| 86 | octomap::point3d closestObst; |
| 87 | float distance; |
| 88 | |
| 89 | distmap.getDistanceAndClosestObstacle(p, distance, closestObst); |
| 90 | |
| 91 | std::cout<<"\n\ndistance at point "<<p.x()<<","<<p.y()<<","<<p.z()<<" is "<<distance<<std::endl; |
| 92 | if(distance < distmap.getMaxDist()) |
| 93 | std::cout<<"closest obstacle to "<<p.x()<<","<<p.y()<<","<<p.z()<<" is at "<<closestObst.x()<<","<<closestObst.y()<<","<<closestObst.z()<<std::endl; |
| 94 | |
| 95 | //if you modify the octree via tree->insertScan() or tree->updateNode() |
| 96 | //just call distmap.update() again to adapt the distance map to the changes made |
| 97 | |
| 98 | delete tree; |
| 99 | |
| 100 | return 0; |
| 101 | } |
nothing calls this directly
no test coverage detected