Added visualization and calculation of obstacles

  * Obstacles get a unique id
  * Obstacles can be refound
  * Obstacles are displayed in rviz