@Override
  protected void initialize() {
    // TODO Auto-generated method stub
    TrajectoryManager.getInstance().registerListener(this);

    List<Waypoint> waypoints = new ArrayList<Waypoint>();
    waypoints.add(new Waypoint(0, 0, 0));
    waypoints.add(new Waypoint(5, 0, 0));

    WaypointManager.getInstance().setWaypointList(new WaypointList(waypoints));
  }
 @Override
 protected void end() {
   // TODO Auto-generated method stub
   TrajectoryManager.getInstance().unregisterListener(this);
   waitingForTrajectory = true;
 }