Maps generated using this repo not playing well with mav_voxblox_planning

[saihv]

Hi, I am trying to generate TSDF maps from Gazebo worlds using this repo (either with the provided building.world or my own custom worlds), which I am then trying to use with the voxblox_rrt_planner repo. While the planner works with its own sample datasets such as shed, rubble etc.; the maps generated using this repo do not work with the planner.

Essentially, the planner complains that no start/goal positions are valid for the robot, although they lie in free space. Most of the time, voxblox reports the output of getMapDistance() for these points as zero. Additionally, when compared to the planner's sample datasets, the maps generated from this repo do not have a significant "traversable" pointcloud. Sample picture here, where the ESDF pointcloud can be seen (green points), along with the "traversability" point cloud (bigger rainbow squares, you can see there are very few). The start and goal locations shown here are considered to be "occupied" by voxblox.

My workflow is as follows:

1. Run voxblox_ground_truth, save TSDF.
2. Run voxblox_rrt_planner and load TSDF, rely on the internal conversion to ESDF.
3. Attempt to use the planner service with the newly generated ESDF.

Am I missing a step when generating these maps? As voxblox_rrt_planner works with the sample datasets provided in its own repo, I decided to post the issue here.

[helenol]

@saihv Thanks for bringing this up! The TSDFs produced by this repo only fill in a thin band within the truncation distance for distances.
The aabb_padding_ dictates how many voxels in each triangle's neighborhood are updated: https://github.com/ethz-asl/voxblox_ground_truth/blob/master/include/voxblox_ground_truth/sdf_creator.h#L43
So theoretically you can crank that up to "big" (and wait aaaaages for your map).

I had some functions to clear all unknown space as "free" in the ESDF computation within some bounding box while developing these things, looks like it may have been a hack and I removed it... These do the same within a sphere: https://github.com/ethz-asl/voxblox/blob/master/voxblox/include/voxblox/utils/planning_utils.h

Assuming you have a watertight mesh though, you should be able to just floodfill from a free point to fix this.
Hmm.
I'm actually in the same exact boat myself of trying to use this the same way... Maybe @victorreijgwart has some inputs of how to get a full ESDF from this?

[helenol]

Ok I spent some time on this, would be awesome if you could give the new settings (floodfill_unoccupied) a try: #4

Cheers!

[victorreijgwart]

@helenol My temporary fix to get ESDFs out of the TSDFs generated by this plugin was to iterate over all voxels of interest (while allocating new blocks where needed), calling

if (std::abs(voxel.weight) < 1e-3) {
    voxel.weight = 1e-3;
    voxel.distance = esdf_integrator_config.default_distance_m;
}

and then running ESDF generation.
Aside from being messy, this somewhat works to validate maps. But it's probably not useful for planning since it doesn't update the voxel weights in a meaningful way and doesn't consider whether voxels are observable by a physical robot starting at a given point.