Test octomap
Put remaining points after table/shelf/object segmentation into octomap and send to planning scene. This should prevent the arm crashing into anything.
Put remaining points after table/shelf/object segmentation into octomap and send to planning scene. This should prevent the arm crashing into anything.