test_octomap_ros.cpp
Go to the documentation of this file.
00001 
00002 #include <octomap/octomap.h>
00003 #include <octomap_ros/OctomapROS.h>
00004 #include <octomap_ros/conversions.h>
00005 #include <tf/transform_datatypes.h>
00006 
00007 using namespace octomap;
00008 using namespace std;
00009 
00010 #define _1DEG 0.01745329251994329577
00011 
00012 int main(int argc, char** argv) {
00013 
00014 
00015   //##############################################################     
00016 
00017   OcTreeROS tree (0.05);
00018 
00019   //  point3d origin (10.01, 10.01, 10.02);
00020   point3d origin (0.01, 0.01, 0.02);
00021   geometry_msgs::Point originPt = pointOctomapToMsg(origin);
00022 
00023   point3d point_on_surface (2.01,0.01,0.01);
00024 
00025   cout << "generating sphere at " << origin << " ..." << endl;
00026 
00027   for (int i=0; i<360; i++) {    
00028     for (int j=0; j<360; j++) {
00029       geometry_msgs::Point endPt = pointOctomapToMsg(origin+point_on_surface);
00030       if (!tree.insertRay(originPt, endPt)) {
00031         cout << "ERROR while inserting ray from " << origin << " to " << point_on_surface << endl;
00032       }
00033       point_on_surface.rotate_IP (0,0,_1DEG);
00034     }
00035     point_on_surface.rotate_IP (0,_1DEG,0);
00036   }
00037 
00038 
00039   cout << "done.\n\n";
00040   tf::Point pointTf(0.0, 0.0, 0.0);
00041   OcTreeROS::NodeType* treeNode = tree.search(pointTf);
00042   if (treeNode)
00043     cout << "Occupancy of node at (0, 0, 0) = " << treeNode->getOccupancy() << " (should be free)\n";
00044   else
00045     cerr << "ERROR: OcTreeNode not found (NULL)\n";
00046 
00047   treeNode = tree.search(tf::Point(10.01, 2.01, 0.01));
00048   if (treeNode)
00049     cout << "Occupancy of node at (0.01, 2.01, 0.01) = " << treeNode->getOccupancy() << " (should be occupied)\n";
00050   else
00051     cerr << "ERROR: OcTreeNode not found (NULL)\n";
00052 
00053   cout << "writing to sphere.bt..." << endl;
00054   tree.octree.writeBinary("sphere.bt");
00055 
00056   // -----------------------------------------------
00057 
00058   cout << "\ncasting rays ..." << endl;
00059 
00060   OcTreeROS sampled_surface(0.05);
00061 
00062   point3d direction = point3d (1.0,0.0,0.0);
00063   point3d obstacle(0,0,0);
00064 
00065   unsigned int hit (0);
00066   unsigned int miss (0);
00067   double mean_dist(0);
00068 
00069   for (int i=0; i<360; i++) {    
00070     for (int j=0; j<360; j++) {
00071       geometry_msgs::Point directionPt = pointOctomapToMsg(direction);
00072       geometry_msgs::Point obstaclePt;
00073       if (!tree.castRay(originPt, directionPt, obstaclePt, true, 3.)) {
00074         miss++;
00075       }
00076       else {
00077         hit++;
00078         point3d obstacle = pointMsgToOctomap(obstaclePt);
00079         mean_dist += (obstacle - origin).norm();
00080         sampled_surface.octree.updateNode(obstacle, true);
00081       }
00082       direction.rotate_IP (0,0,_1DEG);
00083     }
00084     direction.rotate_IP (0,_1DEG,0);
00085   }
00086   cout << "done." << endl;
00087 
00088   mean_dist /= (double) hit;
00089   std::cout << " hits / misses: " << hit  << " / " << miss << std::endl;
00090   std::cout << " mean obstacle dist: " << mean_dist << std::endl;
00091 
00092   cout << "writing sampled_surface.bt" << endl;
00093   sampled_surface.octree.writeBinary("sampled_surface.bt");
00094 
00095 
00096         // -----------------------------------------------
00097         cout << "\ngenerating single rays..." << endl;
00098         OcTreeROS single_beams(0.03333);
00099         int num_beams = 17;
00100         double beamLength = 10.0;
00101         point3d single_origin (1.0, 0.45, 0.45);
00102         geometry_msgs::Point single_originPt = pointOctomapToMsg(single_origin);
00103         point3d single_end(beamLength, 0.0, 0.0);
00104         
00105         
00106         for (int i=0; i<num_beams; i++) {
00107     geometry_msgs::Point single_endPt = pointOctomapToMsg(single_origin+single_end);
00108           if (!single_beams.insertRay(single_originPt, single_endPt)) {
00109                                 cout << "ERROR while inserting ray from " << single_origin << " to " << single_end << endl;
00110           }
00111                         single_end.rotate_IP (0,0,2.0*M_PI/num_beams);
00112                 }
00113         
00114           cout << "done." << endl;
00115                 cout << "writing to beams.bt..." << endl;
00116                 single_beams.octree.writeBinary("beams.bt");
00117 
00118 }


octomap_ros
Author(s): Armin Hornung
autogenerated on Fri Aug 28 2015 11:50:30