Exemplo n.º 1
0
int main(int argc, char** argv) {

  cout << endl;
  cout << "generating example map" << endl;

  OcTree tree (0.1);  // create empty tree with resolution 0.1


  // insert some measurements of occupied cells

  for (int x=-20; x<20; x++) {
    for (int y=-20; y<20; y++) {
      for (int z=-20; z<20; z++) {
        point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
        tree.updateNode(endpoint, true); // integrate 'occupied' measurement
      }
    }
  }

  // insert some measurements of free cells

  for (int x=-30; x<30; x++) {
    for (int y=-30; y<30; y++) {
      for (int z=-30; z<30; z++) {
        point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
        tree.updateNode(endpoint, false);  // integrate 'free' measurement
      }
    }
  }

  cout << endl;
  cout << "performing some queries:" << endl;
  
  point3d query (0., 0., 0.);
  OcTreeNode* result = tree.search (query);
  print_query_info(query, result);

  query = point3d(-1.,-1.,-1.);
  result = tree.search (query);
  print_query_info(query, result);

  query = point3d(1.,1.,1.);
  result = tree.search (query);
  print_query_info(query, result);


  cout << endl;
  tree.writeBinary("simple_tree.bt");
  cout << "wrote example file simple_tree.bt" << endl << endl;
  cout << "now you can use octovis to visualize: octovis simple_tree.bt"  << endl;
  cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl  << endl;  

}
Exemplo n.º 2
0
int main(int argc, char** argv) {


  //##############################################################

  OcTree tree (0.05);
  tree.enableChangeDetection(true);

  point3d origin (0.01f, 0.01f, 0.02f);
  point3d point_on_surface (4.01f,0.01f,0.01f);
  tree.insertRay(origin, point_on_surface);
  printChanges(tree);
  tree.updateNode(point3d(2.01f, 0.01f, 0.01f), 2.0f);
  printChanges(tree);
  tree.updateNode(point3d(2.01f, 0.01f, 0.01f), -2.0f);
  printChanges(tree);

  cout << "generating spherical scan at " << origin << " ..." << endl;

  for (int i=-100; i<101; i++) {
    Pointcloud cloud;
    for (int j=-100; j<101; j++) {
      point3d rotated = point_on_surface;
      rotated.rotate_IP(0, DEG2RAD(i*0.5), DEG2RAD(j*0.5));
      cloud.push_back(rotated);
    }

    // insert in global coordinates:
    tree.insertPointCloud(cloud, origin, -1);
  }

  printChanges(tree);


  cout << "done." << endl;

  return 0;
}
Exemplo n.º 3
0
int main(int argc, char** argv) {

  if (argc != 2){
    std::cerr << "Error: you need to specify a testfile (.bt) as argument to read" << std::endl;
    return 1; // exit 1 means failure
  }

  std::cout << "Testing empty OcTree...\n";
  //empty tree
  {
    OcTree emptyTree(0.999);
    EXPECT_EQ(emptyTree.size(), 0);
    EXPECT_TRUE(emptyTree.writeBinary("empty.bt"));
    EXPECT_TRUE(emptyTree.write("empty.ot"));

    OcTree emptyReadTree(0.2);
    EXPECT_TRUE(emptyReadTree.readBinary("empty.bt"));
    EXPECT_EQ(emptyReadTree.size(), 0);
    EXPECT_TRUE(emptyTree == emptyReadTree);

    
    AbstractOcTree* readTreeAbstract = AbstractOcTree::read("empty.ot");
    EXPECT_TRUE(readTreeAbstract);
    OcTree* readTreeOt = dynamic_cast<OcTree*>(readTreeAbstract);
    EXPECT_TRUE(readTreeOt);  
    EXPECT_EQ(readTreeOt->size(), 0);
    EXPECT_TRUE(emptyTree == *readTreeOt);
    delete readTreeOt;
  }

  std::cout << "Testing reference OcTree from file ...\n";
  string filename = string(argv[1]);

  {
    string filenameOt = "test_io_file.ot";
    string filenameBtOut = "test_io_file.bt";
    string filenameBtCopyOut = "test_io_file_copy.bt";

    // read reference tree from input file
    OcTree tree (0.1);
    EXPECT_TRUE (tree.readBinary(filename));

    std::cout << "    Copy Constructor / assignment / ==\n";
    // test copy constructor / assignment:
    OcTree* treeCopy = new OcTree(tree);
    EXPECT_TRUE(tree == *treeCopy);
    EXPECT_TRUE(treeCopy->writeBinary(filenameBtCopyOut));

    // change a tree property, trees must be different afterwards
    treeCopy->setResolution(tree.getResolution()*2.0);
    EXPECT_FALSE(tree == *treeCopy);
    treeCopy->setResolution(tree.getResolution());
    EXPECT_TRUE(tree == *treeCopy);

    // flip one value, trees must be different afterwards:
    point3d pt(0.5, 0.5, 0.5);
    OcTreeNode* node = treeCopy->search(pt);
    if (node && treeCopy->isNodeOccupied(node))
      treeCopy->updateNode(pt, false);
    else
      treeCopy->updateNode(pt, true);

    EXPECT_FALSE(tree == *treeCopy);


    delete treeCopy;

    std::cout << "    Swap\n";
    // test swap:
    OcTree emptyT(tree.getResolution());
    OcTree emptySw(emptyT);
    OcTree otherSw(tree);
    emptySw.swapContent(otherSw);
    EXPECT_FALSE(emptyT == emptySw);
    EXPECT_TRUE(emptySw == tree);
    EXPECT_TRUE(otherSw == emptyT);


    // write again to bt, read & compare
    EXPECT_TRUE(tree.writeBinary(filenameBtOut));
    OcTree readTreeBt(0.1);
    EXPECT_TRUE(readTreeBt.readBinary(filenameBtOut));
    EXPECT_TRUE(tree == readTreeBt);

    std::cout <<"    Write to .ot / read through AbstractOcTree\n";
    // now write to .ot, read & compare
    EXPECT_TRUE(tree.write(filenameOt));
    
    AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameOt);
    EXPECT_TRUE(readTreeAbstract);

    OcTree* readTreeOt = dynamic_cast<OcTree*>(readTreeAbstract);
    EXPECT_TRUE(readTreeOt);
    EXPECT_TRUE(tree == *readTreeOt);

    // sanity test for "==": flip one node, compare again
    point3d coord(0.1f, 0.1f, 0.1f);
    node = readTreeOt->search(coord);
    if (node && readTreeOt->isNodeOccupied(node))
      readTreeOt->updateNode(coord, false);
    else
      readTreeOt->updateNode(coord, true);

    EXPECT_FALSE(tree == *readTreeOt);
    
    delete readTreeOt;
  }

  // Test for tree headers and IO factory registry (color)
  {
    std::cout << "Testing ColorOcTree...\n";

    double res = 0.02;
    std::string filenameColor = "test_io_color_file.ot";
    ColorOcTree colorTree(res);
    EXPECT_EQ(colorTree.getTreeType(), "ColorOcTree");
    ColorOcTreeNode* colorNode = colorTree.updateNode(point3d(0.0, 0.0, 0.0), true);
    ColorOcTreeNode::Color color_red(255, 0, 0);
    colorNode->setColor(color_red);
    colorTree.setNodeColor(0.0, 0.0, 0.0, 255, 0, 0);
    colorTree.updateNode(point3d(0.1f, 0.1f, 0.1f), true);
    colorTree.setNodeColor(0.1f, 0.1f, 0.1f, 0, 0, 255);

    EXPECT_TRUE(colorTree.write(filenameColor));
    AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameColor);
    EXPECT_TRUE(readTreeAbstract);
    EXPECT_EQ(colorTree.getTreeType(),  readTreeAbstract->getTreeType());
    ColorOcTree* readColorTree = dynamic_cast<ColorOcTree*>(readTreeAbstract);
    EXPECT_TRUE(readColorTree);
    EXPECT_TRUE(colorTree == *readColorTree);
    colorNode = colorTree.search(0.0, 0.0, 0.0);
    EXPECT_TRUE(colorNode);
    EXPECT_EQ(colorNode->getColor(), color_red);
    delete readColorTree;
  }

  // Test for tree headers and IO factory registry (stamped)
  {
    std::cout << "Testing OcTreeStamped...\n";
    double res = 0.05;
    std::string filenameStamped = "test_io_stamped_file.ot";
    OcTreeStamped stampedTree(res);
    EXPECT_EQ(stampedTree.getTreeType(), "OcTreeStamped");
    // TODO: add / modify some stamped nodes
    //ColorOcTreeNode* colorNode = colorTree.updateNode(point3d(0.0, 0.0, 0.0), true);
    //ColorOcTreeNode::Color color_red(255, 0, 0);
    //colorNode->setColor(color_red);
    //colorTree.setNodeColor(0.0, 0.0, 0.0, 255, 0, 0);
    //colorTree.updateNode(point3d(0.1f, 0.1f, 0.1f), true);
    //colorTree.setNodeColor(0.1f, 0.1f, 0.1f, 0, 0, 255);

    EXPECT_TRUE(stampedTree.write(filenameStamped));
    AbstractOcTree* readTreeAbstract = AbstractOcTree::read(filenameStamped);
    EXPECT_TRUE(readTreeAbstract);
    EXPECT_EQ(stampedTree.getTreeType(), readTreeAbstract->getTreeType());
    OcTreeStamped* readStampedTree = dynamic_cast<OcTreeStamped*>(readTreeAbstract);
    EXPECT_TRUE(readStampedTree);
    EXPECT_TRUE(stampedTree == *readStampedTree);
    //colorNode = colorTree.search(0.0, 0.0, 0.0);
    //EXPECT_TRUE(colorNode);
    //EXPECT_EQ(colorNode->getColor(), color_red);    
    
    delete readStampedTree;    
  }


  std::cerr << "Test successful.\n";
  return 0;
}
Exemplo n.º 4
0
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  // Usage:
  //   Constructors/Destructor:
  //    octree = octomapWrapper(resolution);  // constructor: new tree with
  //    specified resolution
  //    octree = octomapWrapper(filename);    // constructor: load from file
  //    octomapWrapper(octree);     // destructor
  //
  //   Queries:
  //    results = octomapWrapper(octree, 1, pts) // search
  //    leaf_nodes = octomapWrapper(octree, 2)  // getLeafNodes
  //
  //   Update tree:
  //    octomapWrapper(octree, 11, pts, occupied)   // updateNote(pts, occupied).
  //    pts is 3-by-n, occupied is 1-by-n logical
  //
  //   General operations:
  //    octomapWrapper(octree, 21, filename)    // save to file

  OcTree* tree = NULL;

  if (nrhs == 1) {
    if (mxIsNumeric(prhs[0])) {  // constructor w/ resolution
      if (nlhs > 0) {
        double resolution = mxGetScalar(prhs[0]);
        //        mexPrintf("Creating octree w/ resolution %f\n", resolution);
        tree = new OcTree(resolution);
        plhs[0] = createDrakeMexPointer((void*)tree, "OcTree");
      }
    } else if (mxIsChar(prhs[0])) {
      if (nlhs > 0) {
        char* filename = mxArrayToString(prhs[0]);
        //        mexPrintf("Loading octree from %s\n", filename);
        tree = new OcTree(filename);
        plhs[0] = createDrakeMexPointer((void*)tree, "OcTree");
        mxFree(filename);
      }
    } else {  // destructor.  note: assumes prhs[0] is a DrakeMexPointer (todo:
              // could check)
              //      mexPrintf("Deleting octree\n");
      destroyDrakeMexPointer<OcTree*>(prhs[0]);
    }
    return;
  }

  tree = (OcTree*)getDrakeMexPointer(prhs[0]);
  int COMMAND = (int)mxGetScalar(prhs[1]);

  switch (COMMAND) {
    case 1:  // search
    {
      mexPrintf("octree search\n");
      if (mxGetM(prhs[2]) != 3)
        mexErrMsgTxt("octomapWrapper: pts must be 3-by-n");
      int n = mxGetN(prhs[2]);
      double* pts = mxGetPrSafe(prhs[2]);
      if (nlhs > 0) {
        plhs[0] = mxCreateDoubleMatrix(1, n, mxREAL);
        double* presults = mxGetPrSafe(plhs[0]);
        for (int i = 0; i < n; i++) {
          OcTreeNode* result =
              tree->search(pts[3 * i], pts[3 * i + 1], pts[3 * i + 2]);
          if (result == NULL)
            presults[i] = -1.0;
          else
            presults[i] = result->getOccupancy();
        }
      }
    } break;
    case 2:  // get leaf nodes
    {
      //      mexPrintf("octree get leaf nodes\n");
      int N = tree->getNumLeafNodes();
      plhs[0] = mxCreateDoubleMatrix(3, N, mxREAL);
      double* leaf_xyz = mxGetPrSafe(plhs[0]);

      double* leaf_value = NULL, * leaf_size = NULL;
      if (nlhs > 1) {  // return value
        plhs[1] = mxCreateDoubleMatrix(1, N, mxREAL);
        leaf_value = mxGetPrSafe(plhs[1]);
      }
      if (nlhs > 2) {  // return size
        plhs[2] = mxCreateDoubleMatrix(1, N, mxREAL);
        leaf_size = mxGetPrSafe(plhs[2]);
      }

      for (OcTree::leaf_iterator leaf = tree->begin_leafs(),
                                 end = tree->end_leafs();
           leaf != end; ++leaf) {
        leaf_xyz[0] = leaf.getX();
        leaf_xyz[1] = leaf.getY();
        leaf_xyz[2] = leaf.getZ();
        leaf_xyz += 3;
        if (leaf_value) *leaf_value++ = leaf->getValue();
        if (leaf_size) *leaf_size++ = leaf.getSize();
      }
    } break;
    case 11:  // add occupied pts
    {
      //        mexPrintf("octree updateNode\n");
      if (mxGetM(prhs[2]) != 3)
        mexErrMsgTxt("octomapWrapper: pts must be 3-by-n");
      int n = mxGetN(prhs[2]);
      double* pts = mxGetPrSafe(prhs[2]);
      mxLogical* occupied = mxGetLogicals(prhs[3]);
      for (int i = 0; i < n; i++) {
        tree->updateNode(pts[3 * i], pts[3 * i + 1], pts[3 * i + 2],
                         occupied[i]);
      }
    } break;
    case 12:  // insert a scan of endpoints and sensor origin
    {
      // pointsA should be 3xN, originA is 3x1
      double* points = mxGetPrSafe(prhs[2]);
      double* originA = mxGetPrSafe(prhs[3]);
      int n = mxGetN(prhs[2]);
      point3d origin((float)originA[0], (float)originA[1], (float)originA[2]);
      Pointcloud pointCloud;
      for (int i = 0; i < n; i++) {
        point3d point((float)points[3 * i], (float)points[3 * i + 1],
                      (float)points[3 * i + 2]);
        pointCloud.push_back(point);
      }
      tree->insertPointCloud(pointCloud, origin);
    } break;
    case 21:  // save to file
    {
      char* filename = mxArrayToString(prhs[2]);
      //        mexPrintf("writing octree to %s\n", filename);
      tree->writeBinary(filename);
      mxFree(filename);
    } break;
    default:
      mexErrMsgTxt("octomapWrapper: Unknown command");
  }
}
Exemplo n.º 5
0
void execute(const fremen::informationGoalConstPtr& goal, Server* as)
{

    /*              Octmap Estimation and Visualization             */

    octomap_msgs::Octomap bmap_msg;
    OcTree octree (resolution);
    geometry_msgs::Point initialPt, finalPt;

    //Create pointcloud:
    octomap::Pointcloud octoCloud;
    sensor_msgs::PointCloud fremenCloud;
    float x = 0*gridPtr->positionX;
    float y = 0*gridPtr->positionY;
    geometry_msgs::Point32 test_point;
    int cnt = 0;

    int cell_x, cell_y, cell_z;
    cell_x = (int)(goal->x/resolution);
    cell_y = (int)(goal->y/resolution);
    cell_z = (int)(head_height/resolution);

    for(double i = LIM_MIN_X; i < LIM_MAX_X; i+=resolution){
        for(double j = LIM_MIN_Y; j < LIM_MAX_Y; j+=resolution){
            for(double w = LIM_MIN_Z; w < LIM_MAX_Z; w+=resolution){
                point3d ptt(x+i+resolution/2,y+j+resolution/2,w+resolution/2);
                int s = goal->stamp;
                if(gridPtr->retrieve(cnt, goal->stamp)>0)
                {
                    //finalPt.z = (int)((w+resolution/2)/resolution)-cell_z;
                    //finalPt.y = (int)((j+resolution/2)/resolution)-cell_y;
                    //finalPt.x = (int)((i+resolution/2)/resolution)-cell_x;

                    //int cnta = ((cell_x+finalPt.x-LIM_MIN_X/resolution)*dim_y + (finalPt.y + cell_y-LIM_MIN_Y/resolution))*dim_z + (finalPt.z + cell_z-LIM_MIN_Z/resolution);
                    //ROS_INFO("something %d %d",cnt,cnta);
                    octoCloud.push_back(x+i+resolution/2,y+j+resolution/2,w+resolution/2);
                    octree.updateNode(ptt,true,true);
                }
                cnt++;
            }
        }
    }
    //Update grid
    octree.updateInnerOccupancy();

    //init visualization markers:
    visualization_msgs::MarkerArray occupiedNodesVis;
    unsigned int m_treeDepth = octree.getTreeDepth();

    //each array stores all cubes of a different size, one for each depth level:
    occupiedNodesVis.markers.resize(m_treeDepth + 1);
    geometry_msgs::Point cubeCenter;

    std_msgs::ColorRGBA m_color;
    m_color.r = 0.0;
    m_color.g = 0.0;
    m_color.b = 1.0;
    m_color.a = 0.5;

    for (unsigned i = 0; i < occupiedNodesVis.markers.size(); ++i)
    {
        double size = octree.getNodeSize(i);
        occupiedNodesVis.markers[i].header.frame_id = "/map";
        occupiedNodesVis.markers[i].header.stamp = ros::Time::now();
        occupiedNodesVis.markers[i].ns = "map";
        occupiedNodesVis.markers[i].id = i;
        occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
        occupiedNodesVis.markers[i].scale.x = size;
        occupiedNodesVis.markers[i].scale.y = size;
        occupiedNodesVis.markers[i].scale.z = size;
        occupiedNodesVis.markers[i].color = m_color;
    }

    ROS_INFO("s %i",cnt++);
    x = gridPtr->positionX;
    y = gridPtr->positionY;


    for(OcTree::leaf_iterator it = octree.begin_leafs(), end = octree.end_leafs(); it != end; ++it)
    {
        if(it != NULL && octree.isNodeOccupied(*it))
        {
            unsigned idx = it.getDepth();
            cubeCenter.x = x+it.getX();
            cubeCenter.y = y+it.getY();
            cubeCenter.z = it.getZ();
            occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
            double minX, minY, minZ, maxX, maxY, maxZ;
            octree.getMetricMin(minX, minY, minZ);
            octree.getMetricMax(maxX, maxY, maxZ);
            double h = (1.0 - fmin(fmax((cubeCenter.z - minZ) / (maxZ - minZ), 0.0), 1.0)) * m_colorFactor;
            occupiedNodesVis.markers[idx].colors.push_back(heightMapColorA(h));
        }
    }

    /**** Robot Head Marker ****/

    //Robot Position


    visualization_msgs::Marker marker_head;
    marker_head.header.frame_id = "/map";
    marker_head.header.stamp = ros::Time();
    marker_head.ns = "my_namespace";
    marker_head.id = 1;
    marker_head.type = visualization_msgs::Marker::SPHERE;
    marker_head.action = visualization_msgs::Marker::ADD;
    marker_head.pose.position.x = goal->x;
    marker_head.pose.position.y = goal->y;
    marker_head.pose.position.z = head_height;
    marker_head.pose.orientation.x = 0.0;
    marker_head.pose.orientation.y = 0.0;
    marker_head.pose.orientation.z = 0.0;
    marker_head.pose.orientation.w = 1.0;
    marker_head.scale.x = 0.2;
    marker_head.scale.y = 0.2;
    marker_head.scale.z = 0.2;
    marker_head.color.a = 1.0;
    marker_head.color.r = 0.0;
    marker_head.color.g = 1.0;
    marker_head.color.b = 0.0;


    /****** Ray Traversal ******/

    //ROS_INFO("Robot Position (%f,%f,%f) = (%d,%d,%d)", goal->x, goal->y, head_height, cell_x, cell_y, cell_z);

    //Ray Casting (Grid Traversal - Digital Differential Analyzer)

    visualization_msgs::Marker marker_rays;
    marker_rays.header.frame_id = "/map";
    marker_rays.header.stamp = ros::Time();
    marker_rays.ns = "my_namespace";
    marker_rays.id = 2;
    marker_rays.type = visualization_msgs::Marker::LINE_LIST;
    marker_rays.action = visualization_msgs::Marker::ADD;
    marker_rays.scale.x = 0.01;


    geometry_msgs::Vector3 rayDirection, deltaT, cellIndex;

    std_msgs::ColorRGBA line_color;

    initialPt.x = goal->x;
    initialPt.y = goal->y;
    initialPt.z = head_height;

    line_color.a = 0.2;
    line_color.r = 0.0;
    line_color.g = 0.0;
    line_color.b = 1.0;

    float delta, H;
    bool free_cell;

    ROS_INFO("Performing Ray Casting...");
            H = 0;
    int gridsize = dim_x*dim_y*dim_z;
    for(float ang_v = -0.174; ang_v < 0.174; ang_v+=angular_step){
        for(float ang_h = 0; ang_h < 2*M_PI; ang_h+= angular_step){ //0 - 360 degrees
            //Initial conditions:
            rayDirection.z = sin(ang_v);//z
            rayDirection.x = cos(ang_v) * cos(ang_h);//x
            rayDirection.y = cos(ang_v) * sin(ang_h);//y

            delta = fmax(fmax(fabs(rayDirection.x), fabs(rayDirection.y)), fabs(rayDirection.z));

            deltaT.x = rayDirection.x/delta;
            deltaT.y = rayDirection.y/delta;
            deltaT.z = rayDirection.z/delta;

            free_cell = true;


            int max_it = RANGE_MAX/resolution * 2/sqrt(pow(deltaT.x,2) + pow(deltaT.y,2) + pow(deltaT.z,2));

            //    ROS_INFO("Max_it: %d %d %d", cell_x,cell_y,cell_z);

            finalPt.x = 0;
            finalPt.y = 0;
            finalPt.z = 0;

            for(int i = 0; i < max_it && free_cell; i++){
                finalPt.x += deltaT.x/2;
                finalPt.y += deltaT.y/2;
                finalPt.z += deltaT.z/2;

                //cnt?
                int cnt = ((int)((cell_x+finalPt.x-LIM_MIN_X/resolution))*dim_y + (int)(finalPt.y + cell_y-LIM_MIN_Y/resolution))*dim_z + (int)(finalPt.z + cell_z-LIM_MIN_Z/resolution);//get fremen grid index!

                //TODO
                if (cnt < 0){
                    cnt = 0;
                    free_cell = false;
                }
                if (cnt > gridsize-1){
                    cnt = gridsize-1;
                    free_cell = false;
                }
                //ROS_INFO("CNT: %d %d", cnt,gridsize);
                //ROS_INFO("DIM %d %d", dim_x, dim_z);
                if(gridPtr->retrieve(cnt, goal->stamp) > 0) free_cell = false;
                if(aux_entropy[cnt] ==0){
                    aux_entropy[cnt] = 1;
                    //float p = gridPtr->estimate(cnt,goal->stamp);
                    //h+=-p*ln(p);
                    H++;
                }
            }


            marker_rays.points.push_back(initialPt);
            marker_rays.colors.push_back(line_color);
            finalPt.x = finalPt.x*resolution+initialPt.x;
            finalPt.y = finalPt.y*resolution+initialPt.y;
            finalPt.z = finalPt.z*resolution+initialPt.z;
            marker_rays.points.push_back(finalPt);
            marker_rays.colors.push_back(line_color);

        }
    }

    //Entropy (text marker):

    visualization_msgs::Marker marker_text;
    marker_text.header.frame_id = "/map";
    marker_text.header.stamp = ros::Time();
    marker_text.ns = "my_namespace";
    marker_text.id = 1;
    marker_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker_text.action = visualization_msgs::Marker::ADD;
    marker_text.pose.position.x = goal->x;
    marker_text.pose.position.y = goal->y;
    marker_text.pose.position.z = head_height + 2;
    marker_text.pose.orientation.x = 0.0;
    marker_text.pose.orientation.y = 0.0;
    marker_text.pose.orientation.z = 0.0;
    marker_text.pose.orientation.w = 1.0;
    marker_text.scale.z = 0.5;
    marker_text.color.a = 1.0;
    marker_text.color.r = 0.0;
    marker_text.color.g = 1.0;
    marker_text.color.b = 0.0;
    char output[1000];
    sprintf(output,"Gain: %f",H);
    marker_text.text = output;


    //Publish Results:
    ROS_INFO("Data published!");
    head_pub_ptr->publish(marker_head);
    estimate_pub_ptr->publish(occupiedNodesVis);
    rays_pub_ptr->publish(marker_rays);
    text_pub_ptr->publish(marker_text);


    as->setSucceeded();

}
Exemplo n.º 6
0
int main(int argc, char** argv) {

  cout << endl;
  cout << "generating example map" << endl;

  OcTree tree (0.1);  // create empty tree with resolution 0.1


  // insert some measurements of occupied cells

  for (int x=-20; x<20; x++) {
    for (int y=-20; y<20; y++) {
      for (int z=-20; z<20; z++) {
        point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
        tree.updateNode(endpoint, true); // integrate 'occupied' measurement
      }
    }
  }

  // insert some measurements of free cells

  for (int x=-30; x<30; x++) {
    for (int y=-30; y<30; y++) {
      for (int z=-30; z<30; z++) {
        point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
        tree.updateNode(endpoint, false);  // integrate 'free' measurement
      }
    }
  }

  cout << endl;
  cout << "performing some queries around the desired voxel:" << endl;
  
  point3d query;
  OcTreeNode* result = NULL;

  for(float z = -0.6; z < -0.21; z += 0.1){
    for(float y = -0.6; y < -0.21; y += 0.1){
      for(float x = -0.6; x < -0.21; x += 0.1){
        query = point3d(x, y, z);
        result = tree.search(query);
        print_query_info(query, result);
      }
    }
  }
  
  query = point3d(-0.5, -0.4, -0.4);
  result = tree.search(query);
	
  vector<point3d> normals;
  if (tree.getNormals(query, normals)){
  
    cout << endl;
    string s_norm = (normals.size() > 1) ? " normals " : " normal ";
    cout << "MC algorithm gives " << normals.size() << s_norm << "in voxel at " << query << endl;
    for(unsigned i = 0; i < normals.size(); ++i)
      cout << "\t" << normals[i].x() << "; " << normals[i].y() << "; " << normals[i].z() << endl;
  } else{
    cout << "query point unknown (no normals)\n";
  }
}