Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Visualization of pruned octomap in RVIZ strange

Hello,

these are two questions, however, they are related.

I have a height map stored in a .xyz file (each line corresponds to an x,y,z pair, like for a point cloud). The resolution for x,y is 2m each. I would like to put this information in an octomap. This means for each x,y, all that is below z is marked as occupied space, everything that is above z is marked as free space.

Currently, I iterate twice through all of the space covered by the octomap. The first time for setting all of the space as free space, and the second time for setting all below z as occupied:

for (double x = 0.; x <= len_z; x += s_x) {

    for (double y = 0.; y <= len_y; y += s_y) {

        for(double z = 0.; z <= len_z; z += s_z) {
            // Create free octomap
            octomap->updateNode(x,y, z, false);
        }

   }

}

for (double x = 0.; x <= len_z; x += s_x) {

    for (double y = 0.; y <= len_y; y += s_y) {

        for(double z = 0.; z <= getZ(x,y); z += s_z) {
            // Fill in info in octomap.
            octomap->updateNode(x,y, z, true);
        }

   }

}
  1. Is there a way to create a free octomap without iterating through all leafs (e.g. assigning free to a node which automatically sets all children as free as well)?

  2. The method described above works. After I created the octomap, I prune it and save it as map.ot. When I visualize it using octovis, everything is fine. However, when I visualize it using RVIZ (see code below), the visualization is bad (no big voxels are shown, just the small ones, I have some screenshots, however I cannot upload pictures). What do I have to do visualize it correctly in RVIZ? (As a note: If I expand the octree (reverse of prune), the visualization is correct, however needs a lot more of computational power.)

    ros::Publisher octomap_pub = n.advertise<octomap_msgs::octomap>("octomap_msg",1);

    octomap_msgs::Octomap octomap_msg;

    octomap_msg.binary = 1;

    octomap_msg.id = 1;

    octomap_msg.header.frame_id = "/base_link";

    octomap_msg.header.stamp = ros::Time::now();

    bool octomap_ok = octomap_msgs::fullMapToMsg(*(mM->octomap),octomap_msg);