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.