Publishing and using obstacle goals

  * Algorithm for finding improved
  * Obstacles have poses now