Problem and solution in OccupancyMapMonitor with multiple sensors

[skuna]

There is a problem in OccupancyMapMonitor::excludeShape() with the mapping between "internal" ShapeHandles (for each OccupancyMapUpdater) and the returned "external" ShapeHandle when using multiple sensors (resp. multiple OccupancyMapUpdaters). This leads to error messages

[ERROR] [1380215318.033656567]: Incorrect mapping of mesh handles
[ERROR] [1380215318.033829969]: Transform cache was not updated. Self-filtering may fail.

of the /move_group node. Octomap data is only visualized for the last configured sensor then.

Solution:
The mesh_handle_count_ should only be incremented once in each call to OccupancyMapMonitor::excludeShape() regardless of the number of updaters.

The appropriate 'patch' is:

--- a/perception/occupancy_map_monitor/src/occupancy_map_monitor.cpp
+++ b/perception/occupancy_map_monitor/src/occupancy_map_monitor.cpp
@@ -216,7 +216,8 @@ ShapeHandle OccupancyMapMonitor::excludeShape(const shapes::ShapeConstPtr &shape
     ShapeHandle mh = map_updaters_[i]->excludeShape(shape);
     if (mh)
     {
-      h = ++mesh_handle_count_;
+      if (h == 0)
+        h = ++mesh_handle_count_;
       mesh_handles_[i][h] = mh;
     }
   }

I hope the format is ok.

This bug was noticed in 0.4.6 but the code until hydro-devel didn't change at this place so the bug should still be present in current versions.

[isucan]

excellent ! thanks for finding this!