Adding simple primitives to ocotmap

[Matthias Mayer]

If I have prior knowledge of free space, say that there is free space in the cube (-1. < x < 0.1, -1 < y < 1, 0 < z < 2) I can add this information to the octomap by the following iterations:

for(float z = -0.1; z <= 1.5; z += 0.01)
    {
        for(float y = -1.5; y <= 1.5; y += 0.01)
        {
            for(float x = -1.5; x <= 1.5; x += 0.01)
            {
                //octomap->updateNode(x, y, z, false);
                if(-0.99 <= x && x <= 0.09 && // subtract 0.01 = resolution / 2 from each boundary to only set nodes truly within the known region
                   -.99 <= y && y <= .99 &&
                   0.01 <= z && z <= 1.49)
                    octomap->setNodeValue(x, y, z, -2.0);

                // add other known primitives here, extend for loops if necessary
            }
        }
    }

This is rather slow and not really using the structure of the octomap.

Therefore I would suggest a approach inspired by the treeIterator:

    std::stack<StackElement,std::vector<StackElement> > s;
    StackElement root;
    root.node = octomap->getRoot();
    root.depth = 0;
    root.key[0] = root.key[1] = root.key[2] = 32768; // 2^15 --> perfect in the middle, I guess
    s.push(root);
   
    octomap->write("preModify.bt");
   
    while(!s.empty()) {
        StackElement here = s.top();
        s.pop();
        ROS_INFO("Entering node with key %d, %d, %d", here.key.k[0], here.key.k[1], here.key.k[2]);
        StackElement tmp;
        tmp.depth = here.depth+1;
        float child_size = octomap->getNodeSize(tmp.depth);
       
        if(here.depth<15) { // highest: 15
            octomap::key_type center_offset_key = 32768 >> tmp.depth;//octomap->tree_max_val >> tmp.depth;
            for(uint8_t i=0; i<8; ++i) {
            // Create all children if not deep enough
                if(!octomap->nodeChildExists(here.node, i)) {
                    octomap->createNodeChild (here.node, i);
                }
                tmp.node = octomap->getNodeChild(here.node, i);
                octomap::computeChildKey(i, center_offset_key, here.key, tmp.key);
                // select children action
                octomap::point3d child_pos = octomap->keyToCoord(tmp.key, tmp.depth);
               
                ROS_INFO("Looking at (%f, %f, %f) with size %f in depth %d", child_pos.x(), child_pos.y(), child_pos.z(), child_size, tmp.depth);
               
                if(-1. <= child_pos.x()-child_size/2 && child_pos.x()+child_size/2 <= 0.1 &&
                    -1. <= child_pos.y()-child_size/2 && child_pos.y()+child_size/2 <= 1. &&
                    0. <= child_pos.z()-child_size/2 && child_pos.z()+child_size/2 <= 1.5) {
                    ROS_INFO("Is inside");
                    octomap->setNodeValue(tmp.key, -2.0); // Node completly within goal area -> set free
                    for(uint8_t j=0; j<8; ++j)
                        if(octomap->nodeChildExists(tmp.node, j)) {
                            ROS_INFO("Killed child %d", j);
                            octomap->deleteNodeChild(tmp.node, j);
                        }
                }
                   
                else if(-1. > child_pos.x()+child_size/2 || child_pos.x()-child_size/2 > 0.1 ||
                    -1. > child_pos.y()+child_size/2 || child_pos.y()-child_size/2 > 1. ||
                    0. > child_pos.z()+child_size/2 || child_pos.z()-child_size/2 > 1.5) {
                    ROS_INFO("Is outside");
                    continue; // ignore node complete outside goal area
                }
               
                else {
                    ROS_INFO("Is split");
                    s.push(tmp); // neither completely in nor out --> must be further subdivided
                }
                   
             }
        } else { // is lowest possible
            child_size*=2;
            octomap::point3d child_pos = octomap->keyToCoord(here.key, here.depth);
            ROS_INFO("Looking at Leaf (%f, %f, %f) with size %f in depth %d", child_pos.x(), child_pos.y(), child_pos.z(), child_size, here.depth);
            if(-1. <= child_pos.x()-child_size/2 && child_pos.x()+child_size/2 <= 0.1 &&
                -1. <= child_pos.y()-child_size/2 && child_pos.y()+child_size/2 <= 1. &&
                0. <= child_pos.z()-child_size/2 && child_pos.z()+child_size/2 <= 1.5) {
                    octomap->setNodeValue(here.key, -2.0); // Node completly within goal area -> set free
                    ROS_INFO("Is inside");
                }
        }
    }

Sadly this approach produces octomaps which seem to be invalid (atleast they cannot be opened by the rviz plugin). Suggestions on a fix or alternative approach would be welcome.