Estimated collision map publishing

[carlosjoserg]

There was really a trouble with the update of the collision map coming from the estimated point cloud and passing through the octomap. I thought of alternating clear point cloud clear octomap routines, but I think it makes more sense to build the collision map within the node and publish from there directly in the processing frame. This would allow to move/plan for the two arms simultaneously to facilitate the planning, otherwise it is a nightmare.

@Tabjones, would be great if you can take a look into the octomap library to publish directly the collision map from the node (in the processing frame), while I continue with the paper. What do you think?

[Tabjones]

@carlosjoserg, ok i dont know how fast this can be, since pcl has an octomap module, but i've have no idea if that is compliant with ros octomap, i'll investigate both and decide what's the best way of doing it