Added state machine for exploration management.

  * Added markers for goals
  * Added goal management