Added debug output info for planning_scene (with Ioan)