Pilot.h

Go to the documentation of this file.

//-*-c++-*-
#ifndef INCLUDED_Pilot_h_
#define INCLUDED_Pilot_h_

// Using a single WaypointWalkMC causes problems if we chain a couple
// of WaypointWalkNodes together because some of the LocomotionEvents
// indicating termination of the first one may not arrive until after
// the second node has been activated, causing the second node to
// terminate prematurely.  The only reason to use shared MCs is for
// the Chiara's XWalk, which runs through an elaborate (slow) leg
// initialization sequence on every start().  No other robots have
// this problem, so the Pilot will only use shared motion commands for
// the Chiara.  For everyone else, waypointwalk_id will be invalid_MC_ID.
#ifdef TGT_IS_CHIARA
#  define PILOT_USES_SHARED_MOTION_COMMANDS
#endif

#include <queue>
#include <deque>

// Have to do all our #includes explicitly; can't rely on
// StateMachine.h because of circular dependencies with the Pilot.

#include "Behaviors/Nodes/HeadPointerNode.h"
#include "Behaviors/Nodes/PostMachine.h"
#include "Behaviors/Nodes/SpeechNode.h"
#include "Behaviors/Nodes/WaypointWalkNode.h"
#include "Behaviors/Nodes/WalkNode.h"

#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/PilotTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Behaviors/Transitions/TextMsgTrans.h"
#include "Behaviors/Transitions/TimeOutTrans.h"

#include "Crew/MapBuilderNode.h"
#include "Crew/PilotRequest.h"

#include "Motion/MMAccessor.h"
#include "Motion/WaypointWalkMC.h"
#include "Motion/WaypointEngine.h"
#include "Motion/WaypointList.h"
#include "Planners/Navigation/ShapeSpacePlannerXY.h"
#include "Planners/Navigation/ShapeSpacePlannerXYTheta.h"

#include "DualCoding/DualCoding.h"

using namespace std;

namespace DualCoding {

class Pilot : public StateNode {
 public:
  Pilot(const std::string &nodename="Pilot") : StateNode(nodename), requests(), curReq(NULL), idCounter(0), walk_id(MotionManager::invalid_MC_ID), waypointwalk_id(MotionManager::invalid_MC_ID), dispatch_(NULL), defaultLandmarks(), defaultLandmarkExtractor(NULL), initPos(), initHead(), planReq(), plan(), maxTurn(M_PI), pushingObj() {}

  typedef unsigned int PilotVerbosity_t;
  static const PilotVerbosity_t PVstart =     1<<0;
  static const PilotVerbosity_t PVevents =    1<<1;
  static const PilotVerbosity_t PVexecute =   1<<2;
  static const PilotVerbosity_t PVsuccess =   1<<3;
  static const PilotVerbosity_t PVfailure =   1<<4;
  static const PilotVerbosity_t PVcomplete =  1<<5;
  static const PilotVerbosity_t PVabort =     1<<6;
  static const PilotVerbosity_t PVpop =       1<<7;
  static const PilotVerbosity_t PVqueued =    1<<8;
  static const PilotVerbosity_t PVcollision = 1<<9;
  static const PilotVerbosity_t PVnavStep =   1<<10;
  static const PilotVerbosity_t PVshowPath =  1<<11;

  static PilotVerbosity_t getVerbosity() { return verbosity; }
  static void setVerbosity(PilotVerbosity_t v) { verbosity = v; }

  /*! @cond INTERNAL */

  enum NavigationStepType_t {
    localizeStep,     //!< localize during path following
    travelStep,       //!< travel forward during path following
    turnStep,         //!< turn to next heading on path
    turnObjStep,      //!< turn while pushing an object
    acquireObjStep    //!< move to acquire contact with object
  };

  struct NavigationStep {
    NavigationStep(NavigationStepType_t _type, DualCoding::Point _waypoint): type(_type), waypoint(_waypoint), visibleLandmarks() {};
    NavigationStepType_t type;
    DualCoding::Point waypoint;
    std::vector<DualCoding::ShapeRoot> visibleLandmarks;
    std::string toString() const;
  };

  //! Stores and indexes into a sequence of actions to complete a navigation task
  struct NavigationPlan {
    NavigationPlan() : path(), steps(), currentStep() {};
    std::vector<DualCoding::Point> path;   //!< Path returned by path planner, from which the plan is generated
    std::vector<NavigationStep> steps;     //!< The plan is a sequence of navigation steps
    std::vector<NavigationStep>::const_iterator currentStep;   //!< Index into the plan
    std::string toString() const;
    void addNavigationStep(NavigationStepType_t type, const DualCoding::Point &waypoint, AngTwoPi orientation);

    void clear() {
      path.clear();
      steps.clear();
      currentStep = steps.begin();
    }
  };

  struct BearingLessThan {
    AngTwoPi hdg;
    BearingLessThan(const AngTwoPi heading) : hdg(heading) {}

    bool operator()(AngTwoPi a, AngTwoPi b) {
      AngTwoPi aBearing = a - hdg;
      AngTwoPi bBearing = b - hdg;
      return aBearing < bBearing;
    }
  };


  virtual void doStart();
  virtual void doStop();
  virtual void doEvent();
  void unwindForNextRequest();
  unsigned int executeRequest(BehaviorBase* requestingBehavior, const PilotRequest& req);
  void executeRequest();
  void requestComplete(PilotTypes::ErrorType_t errorType);
  void requestComplete(const PilotEvent &e);
  void pilotPop();
  void pilotAbort();
  void cancelVelocity(); // forces completion of a setVelocity operation
  void setWorldBounds(const Shape<PolygonData> &bounds);

  void setDefaultLandmarks(const std::vector<ShapeRoot> &landmarks);
  const std::vector<ShapeRoot>& getDefaultLandmarks() { return defaultLandmarks; }

  void setDefaultLandmarkExtractor(const MapBuilderRequest &mapreq);
  MapBuilderRequest* getDefaultLandmarkExtractor() const { return defaultLandmarkExtractor; }

  MotionManager::MC_ID getWalk_id() const { return walk_id; }
  MotionManager::MC_ID getWaypointwalk_id() const { return waypointwalk_id; }

  void setupLandmarkExtractor();
  //! Find the landmarks that should be visible from current location
  static std::vector<ShapeRoot> calculateVisibleLandmarks(const DualCoding::Point &currentLocation, 
                AngTwoPi currentOrientation, AngTwoPi maxTurn,
                const std::vector<DualCoding::ShapeRoot> &possibleLandmarks);
  static bool isLandmarkViewable(const DualCoding::ShapeRoot &selectedLandmark,
         const DualCoding::Point &currentLocation, AngTwoPi currentOrientation, AngTwoPi maxTurn);

  //! Returns true if we have enough landmarks to localize.
  /*! This could be much more sophisticated: taking landmark geometry
   into account so we don't settle for two adjacent landmarks if a
   more widely dispersed set is available.
  */
  static bool defaultLandmarkExitTest();

  //! Pilot's setAgent is called by its RunMotionModel behavior.  Or user can call directly.
  /* If @a quiet is false, the new agent position will be announced on
     cout. If @a updateWayPoint is true, the Pilot's WaypointWalk
     engine's position will also be updated.  This doesn't seem to be a good idea,
     as it leads to fights due to the particle filter preventing the Waypoint Walk
     engine from reaching its destination.  Needs further study.
   */
  void setAgent(const Point &loc, AngTwoPi heading, bool quiet=false, bool updateWaypoint=true);

  //================ Background processes ================

  //! Run the particle filter's motion model every few hundred milliseconds to provide odometry
  class RunMotionModel : public StateNode {
   public:
    RunMotionModel(const std::string &nodename="RunMotionModel") : StateNode(nodename) {}
    virtual void doStart();
    virtual void doEvent();
  };

  //! Collision checker service; its behavior is model-dependent.
  /*! On the Create and Calliope it uses the bump switches and motor torques
   */
  class CollisionChecker : public StateNode {
   public:
    CollisionChecker(const std::string &nodename="CollisionChecker") : StateNode(nodename), overcurrentCounter(0) {}
    virtual void doStart();
    virtual void doEvent();
    virtual void enableDetection();
    virtual void disableDetection(unsigned int howlong);
    virtual void reportCollision();
    int overcurrentCounter; //!< count Create overcurrent button events to eliminate transients
    static unsigned int const collisionEnableTimer = 1;
    static unsigned int const overcurrentResetTimer = 2;
  };

  //================ Utility Classes Used By Pilot State Machines ================

  class ReportCompletion : public StateNode {
   public:
    ReportCompletion(const std::string &nodename, PilotTypes::ErrorType_t _errorType=noError) : StateNode(nodename), errorType(_errorType) {}
    PilotTypes::ErrorType_t errorType;  // cache the constructor's parameter
    virtual void doStart() {
      VRmixin::pilot->requestComplete(errorType);
    }
  };

  //! Used by Pilot walk machines to decide how to respond to a collision
  class CollisionDispatch : public StateNode {
   public:
    CollisionDispatch(const std::string &nodename="CollisionDispatch") : StateNode(nodename) {}
    virtual void doStart();
    virtual void doEvent();
  };

  //! Terminate a walk operation and indicate that a collision was responsible
  class TerminateDueToCollision : public StateNode {
   public:
    TerminateDueToCollision(const std::string &nodename="TerminateDueToCollision") : StateNode(nodename) {}
    virtual void doStart();
  };

  class SetupLandmarkExtractor : public VisualRoutinesStateNode {
   public:
    SetupLandmarkExtractor(const std::string &nodename="SetupLandmarkExtractor") : VisualRoutinesStateNode(nodename) {}
    virtual void doStart() {
        VRmixin::pilot->setupLandmarkExtractor();
    }
  };

  class PlanPath : public VisualRoutinesStateNode {
   public:
    PlanPath(const std::string &nodename="PlanPath") : VisualRoutinesStateNode(nodename) {}
    virtual void doStart() {
    NavigationPlan &plan = getAncestor<Pilot>()->plan;
    Point &initPos = getAncestor<Pilot>()->initPos;
    AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
    PilotRequest &planReq = getAncestor<Pilot>()->planReq;
      plan.clear();
      doPlan(plan, initPos, initHead, planReq);
    }
    void doPlan(NavigationPlan &plan, Point initPos, AngTwoPi initHead, PilotRequest req);
  };

  //! Construct a navigation plan: mixture of moving, turning, and localizing steps
  class ConstructNavPlan : public VisualRoutinesStateNode {
   public:
    ConstructNavPlan(const std::string &nodename="ConstructNavPlan") : VisualRoutinesStateNode(nodename) {}
    virtual void doStart() {
    NavigationPlan &plan = getAncestor<Pilot>()->plan;
    Point &initPos = getAncestor<Pilot>()->initPos;
    AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
    PilotRequest &planReq = getAncestor<Pilot>()->planReq;
      doAnalysis(plan, initPos, initHead, planReq);
    }
    void doAnalysis(NavigationPlan &plan, Point initPos, AngTwoPi initHead, PilotRequest req);
  };

  //! Execute a navigation plan; used for both goToShape and pushObject
  class ExecutePlan : public VisualRoutinesStateNode {
   public:
    ExecutePlan(const std::string &nodename="ExecutePlan") : VisualRoutinesStateNode(nodename), nextDisplacementInstruction(), boxX(), acquireDist() {}

    //! distance in mm we can be from a destination point and be "there"
    static const float allowableDistanceError;
    static const float allowableAngularError;

    //! Each movement step in a navigation plan is a DisplacementInstruction
    struct DisplacementInstruction {
      DisplacementInstruction() : nextPoint(), angleToTurn(0) {};
      fmat::Column<2> nextPoint;
      AngSignPi angleToTurn;
    };

    enum TurnAdjustType_t {
      collisionAdjust,
      localizationAdjust
    };

    class NextTask : public VisualRoutinesStateNode {
     public:
      NextTask(const std::string &nodename="NextTask") : VisualRoutinesStateNode(nodename) {}
      virtual void doStart() {
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
          if ( ++plan.currentStep == plan.steps.end() )
            postStateCompletion();  // indicate that we're done
          else
            postStateSuccess();     // indicate that we have a task to perform
      }
    };

    class Walk : public WaypointWalkNode {
     public:
      Walk(const std::string &nodename="Walk") : WaypointWalkNode(nodename) {}
      virtual void doStart() {
          PilotRequest &req = *VRmixin::pilot->curReq;
        DisplacementInstruction &nextDisplacementInstruction = getAncestor<ExecutePlan>()->nextDisplacementInstruction;
    
          MMAccessor<WaypointWalkMC> wp_acc(getMC());
          wp_acc->getWaypointList().clear();
    
          #ifdef TGT_IS_CREATE
          float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 50.f;
          float va = (req.turnSpeed > 0) ? req.turnSpeed : 1.0f;
          wp_acc->addEgocentricWaypoint(nextDisplacementInstruction.nextPoint.norm(), 0,  0, true, vx, va);
          #else
          float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 15.f;
          float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
          wp_acc->addEgocentricWaypoint(nextDisplacementInstruction.nextPoint.norm(), 0,  0, true, vx, va);
          #endif
    
          motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);        
      }
    };

    class Turn : public WaypointWalkNode {
     public:
      Turn(const std::string &nodename="Turn") : WaypointWalkNode(nodename) {}
      virtual void doStart() {
          PilotRequest &req = *VRmixin::pilot->curReq;
        DisplacementInstruction &nextDisplacementInstruction = getAncestor<ExecutePlan>()->nextDisplacementInstruction;
    
          MMAccessor<WaypointWalkMC> wp_acc(getMC());
          wp_acc->getWaypointList().clear();
          // std::cout<<"turning \n";
    
          #ifdef TGT_IS_CREATE
          float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 50.f;
          float va = (req.turnSpeed > 0) ? req.turnSpeed : 1.0f;
          wp_acc->addEgocentricWaypoint(0, 0, nextDisplacementInstruction.angleToTurn, true, vx, va);
          #else
          // these values are for Chiara
          float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 15.f;
          float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
          wp_acc->addEgocentricWaypoint(0, 0,nextDisplacementInstruction.angleToTurn, true, vx, va);
          #endif
      }
    };

    class TurnObj : public WaypointWalkNode {
     public:
      TurnObj(const std::string &nodename="TurnObj") : WaypointWalkNode(nodename) {}
      virtual void doStart() {
        DisplacementInstruction &nextDisplacementInstruction = getAncestor<ExecutePlan>()->nextDisplacementInstruction;
        float &acquireDist = getAncestor<ExecutePlan>()->acquireDist;
          MMAccessor<WaypointWalkMC> wp_acc(getMC());
          wp_acc->getWaypointList().clear();
    
          const float backup_step_size = 300;  // back up from the box
          wp_acc->addEgocentricWaypoint(-backup_step_size, 0, M_PI, true, 50.f, 1.f );
    
          float theta = nextDisplacementInstruction.angleToTurn;
          float abstheta = fabs(theta);
          float hyp = (backup_step_size + 100)/cos(abstheta);
          float x = (backup_step_size + 100) * tan(abstheta);
          float step2 = (backup_step_size/tan(abstheta)) - 100;
          float d = 500 / sin(abstheta);
    
          cout << "*************** TURN PUSH BOX *****************     " << nextDisplacementInstruction.angleToTurn << "  ******************" << endl;
          if (theta >= 0) {
            //Turn Right
            wp_acc->addEgocentricWaypoint(0, 0, -M_PI/2.f, true, 50.f, 1.0f );
            if (x > 500) {
              wp_acc->addEgocentricWaypoint(500, 0, 0, true, 50.f, 1.0f );
              wp_acc->addEgocentricWaypoint(0, 0, M_PI/2.f, true, 50.f, 1.0f );
              wp_acc->addEgocentricWaypoint(backup_step_size - step2, 0, 0, true, 50.f, 1.0f );
              wp_acc->addEgocentricWaypoint(0, 0, abstheta, true, 50.f, 1.0f );
              // wp_acc->addEgocentricWaypoint(d, 0, 0, true, 50.f, 1.0f );
              acquireDist = d;
            } else {
              wp_acc->addEgocentricWaypoint(x, 0, 0, true, 50.f, 1.0f );
              wp_acc->addEgocentricWaypoint(0, 0, M_PI/2.f + abstheta, true, 50.f, 1.0f );
              //wp_acc->addEgocentricWaypoint(hyp, 0, 0, true, 50.f, 1.0f );
              acquireDist = hyp;
            }
          } else {
            wp_acc->addEgocentricWaypoint(0, 0, -abstheta + M_PI/2.f, true, 50.f, 1.0f );
            wp_acc->addEgocentricWaypoint(backup_step_size*sin(abstheta), 0, 0, true, 50.f, 1.0f );
            wp_acc->addEgocentricWaypoint(0, 0, -M_PI/2.f, true, 50.f, 1.0f );
            wp_acc->addEgocentricWaypoint(backup_step_size*cos(abstheta), 0, 0, true, 50.f, 1.0f );
          }
      }
    };

    class AdjustTurn : public WaypointWalkNode {
     public:
      AdjustTurn(const std::string &nodename, const ExecutePlan::TurnAdjustType_t _adjustmentType) : WaypointWalkNode(nodename), adjustmentType(_adjustmentType) {}
      const ExecutePlan::TurnAdjustType_t adjustmentType;  // cache the constructor's parameter
      virtual void doStart() {
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
          MMAccessor<WaypointWalkMC> wp_acc(getMC());
          wp_acc->getWaypointList().clear();
          DualCoding::Point position = theAgent->getCentroid();
          fmat::Column<3> pos = position.getCoords();
          fmat::Column<3> next = plan.currentStep->waypoint.getCoords();
    
          if (adjustmentType == collisionAdjust) {
            next = plan.currentStep->waypoint.getCoords();
            cout << "*************** ADJUST TURN COLLISION ***********************************" << endl;
          }
          else {
            plan.currentStep++;
            next = plan.currentStep->waypoint.getCoords();
            plan.currentStep--;
            cout << "*************** ADJUST TURN ***********************************" << endl;
          }
    
          fmat::Column<2> disp = fmat::SubVector<2>(next) - fmat::SubVector<2>(pos);
          AngSignPi angle = (atan2(disp[1], disp[0]) - theAgent->getOrientation());
    
          PilotRequest &req = *VRmixin::pilot->curReq;
          #ifdef TGT_IS_CREATE
          float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 50.f;
          float va = (req.turnSpeed > 0) ? req.turnSpeed : 1.0f;
          wp_acc->addEgocentricWaypoint(0, 0, angle, true, vx, va);
          #else
          // these values are for Chiara
          float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 15.f;
          float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
          wp_acc->addEgocentricWaypoint(0, 0, angle, true, vx, va);
          #endif
      }
    };

    class ExecuteStep : public VisualRoutinesStateNode {
     public:
      ExecuteStep(const std::string &nodename="ExecuteStep") : VisualRoutinesStateNode(nodename) {}
        virtual void doStart() {
      DisplacementInstruction &nextDisplacementInstruction = getAncestor<ExecutePlan>()->nextDisplacementInstruction;
      NavigationPlan &plan = getAncestor<Pilot>()->plan;
      bool &pushingObj = getAncestor<Pilot>()->pushingObj;
  
          doExecute(plan, nextDisplacementInstruction, pushingObj);
        }
        virtual void doExecute(NavigationPlan &plan, DisplacementInstruction &nextDisplacementInstruction, const bool pushType);
    };

    class LookForObject : public MapBuilderNode {
     public:
      LookForObject(const std::string &nodename="LookForObject") : MapBuilderNode(nodename) {}
      virtual void doStart() {
          MapBuilderRequest *m = VRmixin::pilot->curReq->objectExtractor;
          if ( m == NULL )
            cancelThisRequest();
          else
            mapreq = *m;
      }
    };

    class SelectObject : public VisualRoutinesStateNode {
     public:
      SelectObject(const std::string &nodename="SelectObject") : VisualRoutinesStateNode(nodename) {}
      virtual void doStart() {
        float &boxX = getAncestor<ExecutePlan>()->boxX;
          PilotRequest &req = *VRmixin::pilot->curReq;
          if ( req.objectMatcher == NULL ) {
            cout << "No objectMatcher specified for pushObject in Pilot" << endl;
            postStateFailure();
          } else {
            ShapeRoot localObj = (*req.objectMatcher)(req.objectShape);
            if ( ! localObj.isValid() )
              postStateFailure();
            else {
              boxX = localObj->getCentroid().coordX();
              postStateCompletion();
            }
          }
      }
    };

    class TurnAndMove : public WaypointWalkNode {
     public:
      TurnAndMove(const std::string &nodename="TurnAndMove") : WaypointWalkNode(nodename) {}
      virtual void doStart() {
        float &acquireDist = getAncestor<ExecutePlan>()->acquireDist;
        float &boxX = getAncestor<ExecutePlan>()->boxX;
          float stepSize = min(acquireDist, 100.f);
          acquireDist -= stepSize;
          float angle;
          angle = asin( (160.f - boxX) * sin(M_PI/6.f)/160.f );  // ***HACK based on 320x240 camera image
          angle = 0;
    
          MMAccessor<WaypointWalkMC> wp_acc(getMC());
          wp_acc->getWaypointList().clear();
    
          #ifdef TGT_IS_CREATE
          float vx = 50.f;
          float va = 1.0f;
          #else
          float vx = 15.f;
          float va = 0.1f;
          #endif
    
          wp_acc->addEgocentricWaypoint(0, 0,  angle, true, vx, va);
          wp_acc->addEgocentricWaypoint(stepSize, 0,  0, true, vx, va);
          motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
      }
    };

    class DecideNode : public VisualRoutinesStateNode {
     public:
      DecideNode(const std::string &nodename="DecideNode") : VisualRoutinesStateNode(nodename) {}
      virtual void doStart() {
        float &acquireDist = getAncestor<ExecutePlan>()->acquireDist;
            if (acquireDist < 1.f)
                postStateCompletion();
            else
                postStateFailure();
      }
    };

    class AcquireObj : public VisualRoutinesStateNode {
     public:
      AcquireObj(const std::string &nodename="AcquireObj") : VisualRoutinesStateNode(nodename) {}
      virtual void doStart() {
        float &acquireDist = getAncestor<ExecutePlan>()->acquireDist;
        DisplacementInstruction &nextDisplacementInstruction = getAncestor<ExecutePlan>()->nextDisplacementInstruction;
            acquireDist = nextDisplacementInstruction.nextPoint.norm();
      }
    };

    virtual void setup() {
     ExecuteStep *execute = new ExecuteStep("execute");
     addNode(execute);

     PostMachineCompletion *postmachinecompletion1 = new PostMachineCompletion("postmachinecompletion1");
     addNode(postmachinecompletion1);

     LocalizationUtility *localizer = new LocalizationUtility("localizer");
     addNode(localizer);

     AdjustTurn *adjust = new AdjustTurn("adjust",localizationAdjust);
     addNode(adjust);
     adjust->setMC(VRmixin::pilot->waypointwalk_id);

     Walk *walker = new Walk("walker");
     addNode(walker);
     walker->setMC(VRmixin::pilot->waypointwalk_id);

     Turn *turner = new Turn("turner");
     addNode(turner);
     turner->setMC(VRmixin::pilot->waypointwalk_id);

     TurnObj *boxturner = new TurnObj("boxturner");
     addNode(boxturner);
     boxturner->setMC(VRmixin::pilot->waypointwalk_id);

     AcquireObj *acquirebox = new AcquireObj("acquirebox");
     addNode(acquirebox);

     LookForObject *locatebox = new LookForObject("locatebox");
     addNode(locatebox);

     SelectObject *select = new SelectObject("select");
     addNode(select);

     TurnAndMove *turnandmove = new TurnAndMove("turnandmove");
     addNode(turnandmove);

     DecideNode *decide = new DecideNode("decide");
     addNode(decide);

     NextTask *next = new NextTask("next");
     addNode(next);

     PostMachineCompletion *postmachinecompletion2 = new PostMachineCompletion("postmachinecompletion2");
     addNode(postmachinecompletion2);

     CollisionDispatch *collisionDispatch = new CollisionDispatch("collisionDispatch");
     addNode(collisionDispatch);

     TerminateDueToCollision *term = new TerminateDueToCollision("term");
     addNode(term);

     startnode = execute;

     execute->addTransition(new SignalTrans<NavigationStepType_t>("signaltrans1",localizer,localizeStep));
     execute->addTransition(new SignalTrans<NavigationStepType_t>("signaltrans2",walker,travelStep));
     execute->addTransition(new SignalTrans<NavigationStepType_t>("signaltrans3",turner,turnStep));
     execute->addTransition(new SignalTrans<NavigationStepType_t>("signaltrans4",turner,turnObjStep));
     execute->addTransition(new SignalTrans<NavigationStepType_t>("signaltrans5",acquirebox,acquireObjStep));
     execute->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans6",next,StateNode::successSignal));
     execute->addTransition(new CompletionTrans("completiontrans1",postmachinecompletion1));
     localizer->addTransition(new CompletionTrans("completiontrans2",adjust));
     localizer->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans7",adjust,StateNode::failureSignal));
     adjust->addTransition(new CompletionTrans("completiontrans3",next));
     walker->addTransition(new CompletionTrans("completiontrans4",next));
     turner->addTransition(new CompletionTrans("completiontrans5",next));
     boxturner->addTransition(new CompletionTrans("completiontrans6",locatebox));
     acquirebox->addTransition(new NullTrans("nulltrans1",locatebox));
     locatebox->addTransition(new CompletionTrans("completiontrans7",select));
     locatebox->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans8",select,StateNode::failureSignal));
     select->addTransition(new NullTrans("nulltrans2",turnandmove));
     turnandmove->addTransition(new CompletionTrans("completiontrans8",decide));
     decide->addTransition(new CompletionTrans("completiontrans9",next));
     decide->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans9",locatebox,StateNode::failureSignal));
     next->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans10",execute,StateNode::successSignal));
     next->addTransition(new CompletionTrans("completiontrans10",postmachinecompletion2));

     PilotTrans *pilottrans1 = new PilotTrans("pilottrans1",term,PilotTypes::collisionDetected);
     collisionDispatch->addTransition(pilottrans1);
     localizer->addTransition(pilottrans1);
     walker->addTransition(pilottrans1);
     turner->addTransition(pilottrans1);
     boxturner->addTransition(pilottrans1);
     turnandmove->addTransition(pilottrans1);

    }
    DisplacementInstruction nextDisplacementInstruction;   // from $provide
    float boxX;   // from $provide
    float acquireDist;   // from $provide
  };

  //================ LocalizationUtility ================

  //! State machine called by various Pilot functions to perform a landmark search and localization.
  class LocalizationUtility : public VisualRoutinesStateNode {
   public:
    LocalizationUtility(const std::string &nodename="LocalizationUtility") : VisualRoutinesStateNode(nodename), initialHeading(), gazePoints(), gazeHeadings() {}


    class TakeInitialPicture : public MapBuilderNode {
     public:
      TakeInitialPicture(const std::string &nodename="TakeInitialPicture") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
      virtual void doStart() {
        AngTwoPi &initialHeading = getAncestor<LocalizationUtility>()->initialHeading;
          initialHeading = VRmixin::theAgent->getOrientation();
        AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
          maxTurn = M_PI;
          if ( VRmixin::pilot->curReq == NULL // needed to prevent crash if behavior is shut down in the ControllerGUI
               || VRmixin::pilot->curReq->landmarkExtractor == NULL ) {
            cancelThisRequest();
            return;
          }
          mapreq = *VRmixin::pilot->curReq->landmarkExtractor;
          mapreq.clearLocal = true;
      }
    };

    class CheckEnoughLandmarks : public VisualRoutinesStateNode {
     public:
      CheckEnoughLandmarks(const std::string &nodename="CheckEnoughLandmarks") : VisualRoutinesStateNode(nodename) {}
      virtual void doStart() {
          if ( VRmixin::pilot->curReq != NULL // needed to prevent crash if behavior is shut down in the ControllerGUI
         && VRmixin::pilot->curReq->landmarkExitTest() )
      postStateSuccess();
          else
      postStateFailure();
      }
    };

#ifdef TGT_HAS_HEAD
    // Use gazepoints since we have a pan/tilt available

    class ChooseGazePoints : public StateNode {
     public:
      ChooseGazePoints(const std::string &nodename="ChooseGazePoints") : StateNode(nodename) {}
        virtual void doStart() {
      std::deque<Point> &gazePoints = getAncestor<LocalizationUtility>()->gazePoints;
    setupGazePoints(gazePoints);
        }
        void setupGazePoints(std::deque<Point> &gazePoints);
    };

    class PrepareForNextGazePoint : public StateNode {
     public:
      PrepareForNextGazePoint(const std::string &nodename="PrepareForNextGazePoint") : StateNode(nodename) {}
        virtual void doStart() {
    // This node just does a quick check because we'll be using the
    // pan/tilt for the actual work.  The no-head version (below)
    // will turn the body.
      std::deque<Point> &gazePoints = getAncestor<LocalizationUtility>()->gazePoints;
    if ( gazePoints.empty() )
      postStateFailure();
    else
      postStateCompletion();
        }
        void setMC(const MotionManager::MC_ID) {}  // dummy for compatibility with headless case below
    };

    class TakeNextPicture : public MapBuilderNode {
     public:
      TakeNextPicture(const std::string &nodename="TakeNextPicture") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
      virtual void doStart() {
          {
      GET_SHAPE(gazePoint, PointData, VRmixin::localShS);
      if ( gazePoint.isValid() )
        VRmixin::localShS.deleteShape(gazePoint);
          }
          // send the MapBuilder to the next gaze point
        std::deque<Point> &gazePoints = getAncestor<LocalizationUtility>()->gazePoints;
          NEW_SHAPE(gazePoint, PointData, new PointData(localShS, gazePoints.front()));
          gazePoints.pop_front();
          mapreq =  *VRmixin::pilot->curReq->landmarkExtractor;
          mapreq.searchArea = gazePoint;
          mapreq.clearLocal = false;
          
      }
    };

    class ReturnToInitialHeading : public StateNode {
     public:
      ReturnToInitialHeading(const std::string &nodename="ReturnToInitialHeading") : StateNode(nodename) {}
        virtual void doStart() {
          // dummy node since we never moved the body (because we have a pan/tilt)
          postStateCompletion();
        }
        void setMC(const MotionManager::MC_ID) {}  // dummy for compatibility with headless case below
    };

#else
    // No pan/tilt available; must turn the body to acquire more landmarks

    class ChooseGazePoints : public StateNode {
     public:
      ChooseGazePoints(const std::string &nodename="ChooseGazePoints") : StateNode(nodename) {}
        virtual void doStart() {
      AngTwoPi &initialHeading = getAncestor<LocalizationUtility>()->initialHeading;
      std::deque<AngTwoPi> &gazeHeadings = getAncestor<LocalizationUtility>()->gazeHeadings;
      AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
  
          setupGazeHeadings(initialHeading, gazeHeadings, maxTurn);
        }
        void setupGazeHeadings(const AngTwoPi initialHeading, std::deque<AngTwoPi> &gazeHeadings, const AngTwoPi maxTurn);
    };

    class PrepareForNextGazePoint : public WaypointWalkNode {
     public:
      PrepareForNextGazePoint(const std::string &nodename="PrepareForNextGazePoint") : WaypointWalkNode(nodename) {}
        virtual void doStart() {
      std::deque<AngTwoPi> &gazeHeadings = getAncestor<LocalizationUtility>()->gazeHeadings;
      AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
          prepareForNext(gazeHeadings, maxTurn);
        }
        void prepareForNext(std::deque<AngTwoPi> &gazeHeadings, const AngTwoPi maxTurn);
    };

    class TakeNextPicture : public MapBuilderNode {
     public:
      TakeNextPicture(const std::string &nodename="TakeNextPicture") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
      virtual void doStart() {
          mapreq =  *VRmixin::pilot->curReq->landmarkExtractor;
          mapreq.clearLocal = false;
        AngTwoPi &initialHeading = getAncestor<LocalizationUtility>()->initialHeading;
          const AngTwoPi curHeading = VRmixin::theAgent->getOrientation();
          const float headingShift = float(curHeading-initialHeading);
          mapreq.baseTransform.rotation() = fmat::rotationZ(headingShift);
    
      }
    };

    class ReturnToInitialHeading : public WaypointWalkNode {
     public:
      ReturnToInitialHeading(const std::string &nodename="ReturnToInitialHeading") : WaypointWalkNode(nodename) {
          setMC(VRmixin::pilot->getWaypointwalk_id());
      }
  
        virtual void doStart() {
          static const float allowableAngularError = 0.10f;
      AngTwoPi &initialHeading = getAncestor<LocalizationUtility>()->initialHeading;
          const AngTwoPi curHeading = VRmixin::theAgent->getOrientation();
          const AngSignPi turnAmount = AngSignPi(initialHeading - curHeading);
          if ( fabs(turnAmount) < allowableAngularError ) {
            postStateCompletion();
            return;
          }
          MMAccessor<WaypointWalkMC> wp_acc(getMC());
          wp_acc->getWaypointList().clear();
  #ifdef TGT_IS_CREATE
          float const va = 0.9f;  // optimal turn speed for CREATE odometry
          wp_acc->addEgocentricWaypoint(0, 0, turnAmount, true, 0, va);
  #else
          // no other headless robots supported yet, so just repeat the Create values for now
          float const va = 0.9f;
          wp_acc->addEgocentricWaypoint(0, 0, turnAmount, true, 0, va);
  #endif
          motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
        }
    };

#endif // TGT_HAS_HEAD

    class Localize : public VisualRoutinesStateNode {
     public:
      Localize(const std::string &nodename="Localize") : VisualRoutinesStateNode(nodename) {}
        static float const minConfidentWeight;
        virtual void doStart();
    };

    virtual void setup() {
     TakeInitialPicture *take = new TakeInitialPicture("take");
     addNode(take);

     CheckEnoughLandmarks *checkEnough = new CheckEnoughLandmarks("checkEnough");
     addNode(checkEnough);

     ChooseGazePoints *choosegazepoints1 = new ChooseGazePoints("choosegazepoints1");
     addNode(choosegazepoints1);

     PrepareForNextGazePoint *loop = new PrepareForNextGazePoint("loop");
     addNode(loop);
     loop->setMC(VRmixin::pilot->getWaypointwalk_id());

     StateNode *statenode1 = new StateNode("statenode1");
     addNode(statenode1);

     TakeNextPicture *takenextpic = new TakeNextPicture("takenextpic");
     addNode(takenextpic);

     CheckEnoughLandmarks *checkEnough2 = new CheckEnoughLandmarks("checkEnough2");
     addNode(checkEnough2);

     ReturnToInitialHeading *returntoinitialheading1 = new ReturnToInitialHeading("returntoinitialheading1");
     addNode(returntoinitialheading1);
     returntoinitialheading1->setMC(VRmixin::pilot->getWaypointwalk_id());

     Localize *loc = new Localize("loc");
     addNode(loc);

     PostMachineSuccess *postmachinesuccess1 = new PostMachineSuccess("postmachinesuccess1");
     addNode(postmachinesuccess1);

     PostMachineCompletion *postmachinecompletion3 = new PostMachineCompletion("postmachinecompletion3");
     addNode(postmachinecompletion3);

     ReturnToInitialHeading *outofoptions = new ReturnToInitialHeading("outofoptions");
     addNode(outofoptions);

     PostMachineFailure *postmachinefailure1 = new PostMachineFailure("postmachinefailure1");
     addNode(postmachinefailure1);

     PostMachineCompletion *postmachinecompletion4 = new PostMachineCompletion("postmachinecompletion4");
     addNode(postmachinecompletion4);

     startnode = take;

     take->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans11",outofoptions,StateNode::failureSignal));
     take->addTransition(new EventTrans("eventtrans1",checkEnough,EventBase::mapbuilderEGID,(size_t)take,EventBase::statusETID));
     checkEnough->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans12",loc,StateNode::successSignal));
     checkEnough->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans13",choosegazepoints1,StateNode::failureSignal));
     choosegazepoints1->addTransition(new NullTrans("nulltrans3",loop));
     loop->addTransition(new CompletionTrans("completiontrans11",statenode1));
     statenode1->addTransition(new TimeOutTrans("timeouttrans1",takenextpic,1000));
     loop->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans14",loop,StateNode::successSignal));
     loop->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans15",outofoptions,StateNode::failureSignal));
     takenextpic->addTransition(new EventTrans("eventtrans2",checkEnough2,EventBase::mapbuilderEGID,(size_t)takenextpic,EventBase::statusETID));
     checkEnough2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans16",returntoinitialheading1,StateNode::successSignal));
     returntoinitialheading1->addTransition(new CompletionTrans("completiontrans12",loc));
     checkEnough2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans17",loop,StateNode::failureSignal));
     loc->addTransition(new NullTrans("nulltrans4",postmachinesuccess1));
     postmachinesuccess1->addTransition(new NullTrans("nulltrans5",postmachinecompletion3));
     outofoptions->addTransition(new CompletionTrans("completiontrans13",postmachinefailure1));
     postmachinefailure1->addTransition(new NullTrans("nulltrans6",postmachinecompletion4));
    }
    AngTwoPi initialHeading;   // from $provide
    std::deque<Point> gazePoints;   // from $provide
    std::deque<AngTwoPi> gazeHeadings;   // from $provide
  };

  //================ LocalizationMachine ================

  class LocalizationMachine : public StateNode {
   public:
    LocalizationMachine(const std::string &nodename="LocalizationMachine") : StateNode(nodename) {}
    virtual void setup() {
     SetupLandmarkExtractor *setuplandmarkextractor1 = new SetupLandmarkExtractor("setuplandmarkextractor1");
     addNode(setuplandmarkextractor1);

     LocalizationUtility *loc = new LocalizationUtility("loc");
     addNode(loc);

     PostMachineSuccess *postmachinesuccess2 = new PostMachineSuccess("postmachinesuccess2");
     addNode(postmachinesuccess2);

     PostMachineFailure *postmachinefailure2 = new PostMachineFailure("postmachinefailure2");
     addNode(postmachinefailure2);

     startnode = setuplandmarkextractor1;

     setuplandmarkextractor1->addTransition(new NullTrans("nulltrans7",loc));
     loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans18",postmachinesuccess2,StateNode::successSignal));
     loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans19",postmachinefailure2,StateNode::failureSignal));
    }
  };

  //================ WalkMachine ================

  class WalkMachine : public StateNode {
   public:
    WalkMachine(const std::string &nodename="WalkMachine") : StateNode(nodename) {}

    class WalkMachine_WaypointWalker : public WaypointWalkNode {
     public:
      WalkMachine_WaypointWalker(const std::string &nodename="WalkMachine_WaypointWalker") : WaypointWalkNode(nodename) {}
        virtual void doStart();
    };

    virtual void setup() {
     startnode = new StateNode("startnode");
     addNode(startnode);

     WalkMachine_WaypointWalker *walker = new WalkMachine_WaypointWalker("walker");
     addNode(walker);
     walker->setMC(VRmixin::pilot->getWaypointwalk_id());

     ReportCompletion *reportcompletion1 = new ReportCompletion("reportcompletion1");
     addNode(reportcompletion1);

     CollisionDispatch *collisionDispatch = new CollisionDispatch("collisionDispatch");
     addNode(collisionDispatch);

     TerminateDueToCollision *terminateduetocollision1 = new TerminateDueToCollision("terminateduetocollision1");
     addNode(terminateduetocollision1);

     startnode = startnode;

     NullTrans *nulltrans8 = new NullTrans("nulltrans8",walker);
     nulltrans8->addDestination(collisionDispatch);
     startnode->addTransition(nulltrans8);

     walker->addTransition(new CompletionTrans("completiontrans14",reportcompletion1));

     PilotTrans *pilottrans2 = new PilotTrans("pilottrans2",terminateduetocollision1,PilotTypes::collisionDetected);
     collisionDispatch->addTransition(pilottrans2);
     walker->addTransition(pilottrans2);

    }

  };

  //================ WaypointWalkMachine ================

  class WaypointWalkMachine : public StateNode {
   public:
    WaypointWalkMachine(const std::string &nodename="WaypointWalkMachine") : StateNode(nodename) {}

    class WaypointWalkMachine_WaypointWalker : public WaypointWalkNode {
     public:
      WaypointWalkMachine_WaypointWalker(const std::string &nodename="WaypointWalkMachine_WaypointWalker") : WaypointWalkNode(nodename) {}
        virtual void doStart();
    };

    virtual void setup() {
     startnode = new StateNode("startnode");
     addNode(startnode);

     WaypointWalkMachine_WaypointWalker *walker = new WaypointWalkMachine_WaypointWalker("walker");
     addNode(walker);
     walker->setMC(VRmixin::pilot->getWaypointwalk_id());

     ReportCompletion *reportcompletion2 = new ReportCompletion("reportcompletion2");
     addNode(reportcompletion2);

     CollisionDispatch *collisionDispatch = new CollisionDispatch("collisionDispatch");
     addNode(collisionDispatch);

     TerminateDueToCollision *terminateduetocollision2 = new TerminateDueToCollision("terminateduetocollision2");
     addNode(terminateduetocollision2);

     startnode = startnode;

     NullTrans *nulltrans9 = new NullTrans("nulltrans9",walker);
     nulltrans9->addDestination(collisionDispatch);
     startnode->addTransition(nulltrans9);

     walker->addTransition(new CompletionTrans("completiontrans15",reportcompletion2));

     PilotTrans *pilottrans3 = new PilotTrans("pilottrans3",terminateduetocollision2,PilotTypes::collisionDetected);
     collisionDispatch->addTransition(pilottrans3);
     walker->addTransition(pilottrans3);

    }

  };

  //=============== SetVelocityMachine ==================
  class SetVelocityMachine : public StateNode {
   public:
    SetVelocityMachine(const std::string &nodename="SetVelocityMachine") : StateNode(nodename) {}

    class SetVelocityMachine_Walker : public WalkNode {
     public:
      SetVelocityMachine_Walker(const std::string &nodename="SetVelocityMachine_Walker") : WalkNode(nodename) {}
        virtual void doStart();
    };

    virtual void setup() {
     startnode = new StateNode("startnode");
     addNode(startnode);

     SetVelocityMachine_Walker *walker = new SetVelocityMachine_Walker("walker");
     addNode(walker);
     walker->setMC(VRmixin::pilot->getWalk_id());

     StateNode *statenode2 = new StateNode("statenode2");
     addNode(statenode2);

     ReportCompletion *reportcompletion3 = new ReportCompletion("reportcompletion3");
     addNode(reportcompletion3);

     CollisionDispatch *collisionDispatch = new CollisionDispatch("collisionDispatch");
     addNode(collisionDispatch);

     TerminateDueToCollision *terminateduetocollision3 = new TerminateDueToCollision("terminateduetocollision3");
     addNode(terminateduetocollision3);

     startnode = startnode;

     NullTrans *nulltrans10 = new NullTrans("nulltrans10",walker);
     nulltrans10->addDestination(collisionDispatch);
     startnode->addTransition(nulltrans10);

     walker->addTransition(new CompletionTrans("completiontrans16",statenode2));
     statenode2->addTransition(new TimeOutTrans("timeouttrans2",reportcompletion3,2000));

     PilotTrans *pilottrans4 = new PilotTrans("pilottrans4",terminateduetocollision3,PilotTypes::collisionDetected);
     collisionDispatch->addTransition(pilottrans4);
     walker->addTransition(pilottrans4);

    }

  };

  //================ GoToShapeMachine ================

  class GoToShapeMachine : public VisualRoutinesStateNode {
   public:
    GoToShapeMachine(const std::string &nodename="GoToShapeMachine") : VisualRoutinesStateNode(nodename) {}

    //! Check that the request's goToShape parameters are valid
    class CheckParameters : public StateNode {
     public:
      CheckParameters(const std::string &nodename="CheckParameters") : StateNode(nodename) {}
        virtual void doStart();
    };

    class SetUpPlanNode : public StateNode {
     public:
      SetUpPlanNode(const std::string &nodename="SetUpPlanNode") : StateNode(nodename) {}
      virtual void doStart() {
        Point &initPos = getAncestor<Pilot>()->initPos;
        AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
        PilotRequest &planReq = getAncestor<Pilot>()->planReq;
        bool &pushingObj = getAncestor<Pilot>()->pushingObj;
    
            initPos = theAgent->getCentroid();
            initHead = theAgent->getOrientation();
            planReq = *VRmixin::pilot->curReq;
            pushingObj = false;
      }
    };

    class SetMaxTurn : public StateNode {
     public:
      SetMaxTurn(const std::string &nodename="SetMaxTurn") : StateNode(nodename) {}
      virtual void doStart() {
        AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
            maxTurn = M_PI;
      }
    };

    virtual void  setup() {   // setup GoToShapeMachine

     SetMaxTurn *setmaxturn1 = new SetMaxTurn("setmaxturn1");
     addNode(setmaxturn1);

     CheckParameters *check = new CheckParameters("check");
     addNode(check);

     ReportCompletion *reportcompletion4 = new ReportCompletion("reportcompletion4",invalidRequest);
     addNode(reportcompletion4);

     SetupLandmarkExtractor *setuplandmarkextractor2 = new SetupLandmarkExtractor("setuplandmarkextractor2");
     addNode(setuplandmarkextractor2);

     LocalizationUtility *loc = new LocalizationUtility("loc");
     addNode(loc);

     SetUpPlanNode *setupplan = new SetUpPlanNode("setupplan");
     addNode(setupplan);

     PlanPath *planpath = new PlanPath("planpath");
     addNode(planpath);

     ReportCompletion *reportcompletion5 = new ReportCompletion("reportcompletion5",startCollides);
     addNode(reportcompletion5);

     ReportCompletion *reportcompletion6 = new ReportCompletion("reportcompletion6",endCollides);
     addNode(reportcompletion6);

     ReportCompletion *reportcompletion7 = new ReportCompletion("reportcompletion7",noPath);
     addNode(reportcompletion7);

     ReportCompletion *reportcompletion8 = new ReportCompletion("reportcompletion8");
     addNode(reportcompletion8);

     CollisionDispatch *collisionDispatch = new CollisionDispatch("collisionDispatch");
     addNode(collisionDispatch);

     ConstructNavPlan *constructnavplan = new ConstructNavPlan("constructnavplan");
     addNode(constructnavplan);

     ExecutePlan *execute = new ExecutePlan("execute");
     addNode(execute);

     PostMachineCompletion *postmachinecompletion5 = new PostMachineCompletion("postmachinecompletion5");
     addNode(postmachinecompletion5);

     TerminateDueToCollision *term = new TerminateDueToCollision("term");
     addNode(term);

     startnode = setmaxturn1;

     setmaxturn1->addTransition(new NullTrans("nulltrans11",check));
     check->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans20",reportcompletion4,StateNode::failureSignal));
     check->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans21",setuplandmarkextractor2,StateNode::successSignal));
     setuplandmarkextractor2->addTransition(new NullTrans("nulltrans12",loc));
     loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans22",setupplan,StateNode::successSignal));
     loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans23",setupplan,StateNode::failureSignal));
     setupplan->addTransition(new NullTrans("nulltrans13",planpath));
     planpath->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans24",reportcompletion5,GenericRRTBase::START_COLLIDES));
     planpath->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans25",reportcompletion6,GenericRRTBase::END_COLLIDES));
     planpath->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans26",reportcompletion7,GenericRRTBase::MAX_ITER));
     planpath->addTransition(new CompletionTrans("completiontrans17",constructnavplan));
     planpath->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans27",reportcompletion8,StateNode::failureSignal));

     CompletionTrans *completiontrans18 = new CompletionTrans("completiontrans18",execute);
     completiontrans18->addDestination(collisionDispatch);
     constructnavplan->addTransition(completiontrans18);

     execute->addTransition(new CompletionTrans("completiontrans19",postmachinecompletion5));

     PilotTrans *pilottrans5 = new PilotTrans("pilottrans5",term,PilotTypes::collisionDetected);
     collisionDispatch->addTransition(pilottrans5);
     execute->addTransition(pilottrans5);

    }
  };

  //================ PushObjectMachine ================

  class PushObjectMachine : public VisualRoutinesStateNode {
   public:
    PushObjectMachine(const std::string &nodename="PushObjectMachine") : VisualRoutinesStateNode(nodename), initToObjPlan(), objToDestPlan() {}



    class SetMaxTurn : public StateNode {
     public:
      SetMaxTurn(const std::string &nodename, float _value=M_PI) : StateNode(nodename), value(_value) {}
      float value;  // cache the constructor's parameter
      virtual void doStart() {
        AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
          maxTurn = value;
      }
    };

    class CheckParameters : public StateNode {
     public:
      CheckParameters(const std::string &nodename="CheckParameters") : StateNode(nodename) {}
        virtual void doStart();
    };

    class SetUpObjToDest : public StateNode {
     public:
      SetUpObjToDest(const std::string &nodename="SetUpObjToDest") : StateNode(nodename) {}
      virtual void doStart() {
        Point &initPos = getAncestor<Pilot>()->initPos;
        PilotRequest &planReq = getAncestor<Pilot>()->planReq;
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
        bool &pushingObj = getAncestor<Pilot>()->pushingObj;
          plan.clear();
          initPos = VRmixin::pilot->curReq->objectShape->getCentroid();
          planReq = *VRmixin::pilot->curReq;
          planReq.objectShape->setObstacle(false);
          pushingObj = false;
      }
    };

    class BackOffDest : public StateNode {
     public:
      BackOffDest(const std::string &nodename="BackOffDest") : StateNode(nodename) {}
      virtual void doStart() {
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
          size_t s = plan.path.size();
          Point final = plan.path[s-1];
          Point prev = plan.path[s-2];
          Point vec = final-prev;
          float len = vec.xyNorm();
          float robotDiam = 150;  // *** Should take from bounding box
          // *** should also subtract half of object diameter
          // back off by robot diameter to leave object at the destination
          if ( len > robotDiam )
            plan.path[s-1] = prev + vec/len*(len-robotDiam);
      }
    };

    class AddAcquirePt : public StateNode {
     public:
      AddAcquirePt(const std::string &nodename="AddAcquirePt") : StateNode(nodename) {}
      virtual void doStart() {
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
          const Point delta = (plan.path[1] - plan.path[0]); 
          const Point deltaStep = delta.unitVector() * (-400);  // *** HACK: should compute from robot bounding box
          // *** HACK: should check this segment for collisions; don't assume it's safe
          const Point acquirePt = plan.path[0] + deltaStep;
    
          std::vector<DualCoding::Point>::iterator it;
          it = plan.path.begin();
          plan.path.insert(it, acquirePt);
    
          // Display path from object to destination
          NEW_SHAPE(plannedObjToDestPath, GraphicsData, new GraphicsData(worldShS));
          GraphicsData::xyPair U, V;
          for ( size_t i = 1; i < plan.path.size(); i++ ) {
            U.first = plan.path[i-1].coordX(); U.second = plan.path[i-1].coordY();
            V.first = plan.path[i].coordX(); V.second = plan.path[i].coordY();
            plannedObjToDestPath->add(new GraphicsData::LineElement(U, V, rgb(0,255,255)));
          }
      }
    };

    class SetUpInitToObj : public StateNode {
     public:
      SetUpInitToObj(const std::string &nodename="SetUpInitToObj") : StateNode(nodename) {}
      virtual void doStart() {
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
        Point &initPos = getAncestor<Pilot>()->initPos;
        AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
        PilotRequest &planReq = getAncestor<Pilot>()->planReq;
        NavigationPlan &objToDestPlan = getAncestor<PushObjectMachine>()->objToDestPlan;
          objToDestPlan.clear();
          objToDestPlan.path = plan.path;
          plan.clear();
    
          initPos = theAgent->getCentroid();
          initHead = theAgent->getOrientation();
          planReq = *VRmixin::pilot->curReq;
          planReq.objectShape->setObstacle(true);
    
          NEW_SHAPE( dest, PointData, new PointData(worldShS, plan.path[0] ) );
          dest->setObstacle(false);
          planReq.targetShape = dest;
      }
    };

    class Walk : public WaypointWalkNode {
     public:
      Walk(const std::string &nodename="Walk") : WaypointWalkNode(nodename) {}
      virtual void doStart() {
          MMAccessor<WaypointWalkMC> wp_acc(getMC());
          wp_acc->getWaypointList().clear();
    
          #ifdef TGT_IS_CREATE
          wp_acc->addEgocentricWaypoint(-300, 0,  0, true, 100.f, 0.f);
          #else
          wp_acc->addEgocentricWaypoint(-300, 0,  0, true, 100.f, 0.f);
          #endif
          motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);        
      }
    };

    class SetUpExecute2 : public StateNode {
     public:
      SetUpExecute2(const std::string &nodename="SetUpExecute2") : StateNode(nodename) {}
      virtual void doStart() {
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
        Point &initPos = getAncestor<Pilot>()->initPos;
        AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
        PilotRequest &planReq = getAncestor<Pilot>()->planReq;
        AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
        bool &pushingObj = getAncestor<Pilot>()->pushingObj;
        NavigationPlan &objToDestPlan = getAncestor<PushObjectMachine>()->objToDestPlan;
        NavigationPlan &initToObjPlan = getAncestor<PushObjectMachine>()->initToObjPlan;
    
          initToObjPlan.clear();
          initToObjPlan.path = plan.path;
          initToObjPlan.steps = plan.steps;
    
          plan.clear();
          plan.path = objToDestPlan.path;
    
          PilotRequest &req = *VRmixin::pilot->curReq;
          initPos = theAgent->getCentroid();
          initHead = theAgent->getOrientation();
          req.turnSpeed = 0.25f;
          req.forwardSpeed = 50.f;
          planReq = req;
          maxTurn = 0.f;
          pushingObj = true;
          req.collisionAction = PilotTypes::collisionIgnore;
      }
    };

    class MoveTheObject : public VisualRoutinesStateNode {
     public:
      MoveTheObject(const std::string &nodename="MoveTheObject") : VisualRoutinesStateNode(nodename) {}
      virtual void doStart() {
          const Point destination = VRmixin::pilot->curReq->targetShape->getCentroid();
          // *** do we need a setLocation method in BaseData?
          if ( VRmixin::pilot->curReq->objectShape->isType(ellipseDataType) )
            ShapeRootType(VRmixin::pilot->curReq->objectShape,EllipseData)->setCentroidPt(destination);
      }
    };

    virtual void setup() { // set up PushObjectMachine
     SetMaxTurn *setmaxturn2 = new SetMaxTurn("setmaxturn2");
     addNode(setmaxturn2);

     CheckParameters *checkparams = new CheckParameters("checkparams");
     addNode(checkparams);

     ReportCompletion *reportcompletion9 = new ReportCompletion("reportcompletion9",invalidRequest);
     addNode(reportcompletion9);

     SetupLandmarkExtractor *localize = new SetupLandmarkExtractor("localize");
     addNode(localize);

     LocalizationUtility *locutil = new LocalizationUtility("locutil");
     addNode(locutil);

     SetUpObjToDest *plandelivery = new SetUpObjToDest("plandelivery");
     addNode(plandelivery);

     PlanPath *planpath2 = new PlanPath("planpath2");
     addNode(planpath2);

     ReportCompletion *reportcompletion10 = new ReportCompletion("reportcompletion10",startCollides);
     addNode(reportcompletion10);

     ReportCompletion *reportcompletion11 = new ReportCompletion("reportcompletion11",endCollides);
     addNode(reportcompletion11);

     ReportCompletion *reportcompletion12 = new ReportCompletion("reportcompletion12",noPath);
     addNode(reportcompletion12);

     ReportCompletion *reportcompletion13 = new ReportCompletion("reportcompletion13");
     addNode(reportcompletion13);

     BackOffDest *planacquire = new BackOffDest("planacquire");
     addNode(planacquire);

     AddAcquirePt *addacquirept1 = new AddAcquirePt("addacquirept1");
     addNode(addacquirept1);

     SetUpInitToObj *setupinittoobj1 = new SetUpInitToObj("setupinittoobj1");
     addNode(setupinittoobj1);

     PlanPath *planpath1 = new PlanPath("planpath1");
     addNode(planpath1);

     ReportCompletion *reportcompletion14 = new ReportCompletion("reportcompletion14",startCollides);
     addNode(reportcompletion14);

     ReportCompletion *reportcompletion15 = new ReportCompletion("reportcompletion15",endCollides);
     addNode(reportcompletion15);

     ReportCompletion *reportcompletion16 = new ReportCompletion("reportcompletion16",noPath);
     addNode(reportcompletion16);

     ConstructNavPlan *constructnavplan1 = new ConstructNavPlan("constructnavplan1");
     addNode(constructnavplan1);

     ReportCompletion *reportcompletion17 = new ReportCompletion("reportcompletion17");
     addNode(reportcompletion17);

     ExecutePlan *execute = new ExecutePlan("execute");
     addNode(execute);

     SetUpExecute2 *setupexecute21 = new SetUpExecute2("setupexecute21");
     addNode(setupexecute21);

     ConstructNavPlan *constructnavplan2 = new ConstructNavPlan("constructnavplan2");
     addNode(constructnavplan2);

     ExecutePlan *executeplan1 = new ExecutePlan("executeplan1");
     addNode(executeplan1);

     MoveTheObject *movetheobject1 = new MoveTheObject("movetheobject1");
     addNode(movetheobject1);

     Walk *walk1 = new Walk("walk1");
     addNode(walk1);
     walk1->setMC(VRmixin::pilot->waypointwalk_id);

     ReportCompletion *reportcompletion18 = new ReportCompletion("reportcompletion18");
     addNode(reportcompletion18);

     startnode = setmaxturn2;

     setmaxturn2->addTransition(new NullTrans("nulltrans14",checkparams));
     checkparams->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans28",localize,StateNode::successSignal));
     checkparams->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans29",reportcompletion9,StateNode::failureSignal));
     localize->addTransition(new NullTrans("nulltrans15",locutil));
     locutil->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans30",plandelivery,StateNode::successSignal));
     locutil->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans31",plandelivery,StateNode::failureSignal));
     plandelivery->addTransition(new NullTrans("nulltrans16",planpath2));
     planpath2->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans32",reportcompletion10,GenericRRTBase::START_COLLIDES));
     planpath2->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans33",reportcompletion11,GenericRRTBase::END_COLLIDES));
     planpath2->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans34",reportcompletion12,GenericRRTBase::MAX_ITER));
     planpath2->addTransition(new CompletionTrans("completiontrans20",planacquire));
     planpath2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans35",reportcompletion13,StateNode::failureSignal));
     planacquire->addTransition(new NullTrans("nulltrans17",addacquirept1));
     addacquirept1->addTransition(new NullTrans("nulltrans18",setupinittoobj1));
     setupinittoobj1->addTransition(new NullTrans("nulltrans19",planpath1));
     planpath1->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans36",reportcompletion14,GenericRRTBase::START_COLLIDES));
     planpath1->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans37",reportcompletion15,GenericRRTBase::END_COLLIDES));
     planpath1->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans38",reportcompletion16,GenericRRTBase::MAX_ITER));
     planpath1->addTransition(new CompletionTrans("completiontrans21",constructnavplan1));
     constructnavplan1->addTransition(new CompletionTrans("completiontrans22",execute));
     planpath1->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans39",reportcompletion17,StateNode::failureSignal));
     execute->addTransition(new CompletionTrans("completiontrans23",setupexecute21));
     setupexecute21->addTransition(new NullTrans("nulltrans20",constructnavplan2));
     constructnavplan2->addTransition(new CompletionTrans("completiontrans24",executeplan1));
     executeplan1->addTransition(new CompletionTrans("completiontrans25",movetheobject1));
     movetheobject1->addTransition(new NullTrans("nulltrans21",walk1));
     walk1->addTransition(new CompletionTrans("completiontrans26",reportcompletion18));
    }

    NavigationPlan initToObjPlan;   // from $provide
    NavigationPlan objToDestPlan;   // from $provide
  };

  //================ VisualSearchMachine ================

  class VisualSearchMachine : public StateNode {
   public:
    VisualSearchMachine(const std::string &nodename="VisualSearchMachine") : StateNode(nodename) {}

    class Search : public MapBuilderNode {
     public:
      Search(const std::string &nodename="Search") : MapBuilderNode(nodename) {}
      virtual void doStart() {
          if (VRmixin::pilot->curReq->searchObjectExtractor == NULL) {
      cout << "Pilot received visualSearch request with invalid searchObjectExtractor field" << endl;
      VRmixin::pilot->requestComplete(invalidRequest);
          }
          else if (VRmixin::pilot->curReq->searchExitTest == NULL) {
      cout << "Pilot received visualSearch request with no exitTest" << endl;
      VRmixin::pilot->requestComplete(invalidRequest);
          }
          mapreq = *VRmixin::pilot->curReq->searchObjectExtractor;
      }
    };

    class Check : public StateNode {
     public:
      Check(const std::string &nodename="Check") : StateNode(nodename) {}
      virtual void doStart() {
          if (VRmixin::pilot->curReq->searchExitTest())
      VRmixin::pilot->requestComplete(noError);
          else if (VRmixin::pilot->curReq->searchRotationAngle == 0)
      VRmixin::pilot->requestComplete(searchFailed);
          else
      postStateCompletion();
      }
    };

    class Rotate : public WaypointWalkNode {
     public:
      Rotate(const std::string &nodename="Rotate") : WaypointWalkNode(nodename) {}
      virtual void doStart() {
          MMAccessor<WaypointWalkMC> wp_acc(getMC());
          wp_acc->getWaypointList().clear();
          wp_acc->addEgocentricWaypoint(0,0,VRmixin::pilot->curReq->searchRotationAngle,true,0.01f);
      }
    };

    virtual void setup() {
     startnode = new Search("startnode");
     addNode(startnode);

     Check *check1 = new Check("check1");
     addNode(check1);

     Rotate *rotate1 = new Rotate("rotate1");
     addNode(rotate1);
     rotate1->setMC(VRmixin::pilot->waypointwalk_id);

     startnode = startnode;

     startnode->addTransition(new EventTrans("eventtrans3",check1,EventBase::mapbuilderEGID,(size_t)startnode,EventBase::statusETID));
     check1->addTransition(new CompletionTrans("completiontrans27",rotate1));
     rotate1->addTransition(new CompletionTrans("completiontrans28",startnode));
    }
  };



  //================ Dispatch ================
  // All the Pilot's little state machines are children of the
  // Dispatch node, so when a call to Dispatch::finish() causes it to
  // complete and exit, everything gets shut down.

  class Dispatch : public StateNode {
   public:
    Dispatch(const std::string &nodename="Dispatch") : StateNode(nodename), localizeMachine_(NULL), walkMachine_(NULL), waypointWalkMachine_(NULL), setVelocityMachine_(NULL), goToShapeMachine_(NULL), pushObjectMachine_(NULL), visualSearchMachine_(NULL) {}

      StateNode *localizeMachine_, *walkMachine_, *waypointWalkMachine_, *setVelocityMachine_, 
  *goToShapeMachine_, *pushObjectMachine_, *visualSearchMachine_;

    void finish() {
      postStateCompletion();
    }

    virtual void doStart() {
      PilotTypes::RequestType_t reqType = VRmixin::pilot->curReq->getRequestType();
      switch ( reqType) {
      case localize:
        localizeMachine_->start();
        break;
      case walk:
        walkMachine_->start();
        break;
      case waypointWalk:
        waypointWalkMachine_->start();
        break;
      case setVelocity:
        setVelocityMachine_->start();
        break;
      case goToShape:
        goToShapeMachine_->start();
        break;
      case pushObject:
        pushObjectMachine_->start();
        break;
      case visualSearch:
        visualSearchMachine_->start();
        break;
      default:
        cout << "Illegal Pilot request type: " << reqType << endl;
        VRmixin::pilot->requestComplete(invalidRequest);
      }
    };

    virtual void setup(){
     StateNode *dispatchDummyStart = new StateNode("dispatchDummyStart");
     addNode(dispatchDummyStart);

     ReportCompletion *reportSuccess = new ReportCompletion("reportSuccess");
     addNode(reportSuccess);

     ReportCompletion *reportFailure = new ReportCompletion("reportFailure",someError);
     addNode(reportFailure);

     LocalizationMachine *localizemachine = new LocalizationMachine("localizemachine");
     addNode(localizemachine);

     ReportCompletion *reportcompletion19 = new ReportCompletion("reportcompletion19",cantLocalize);
     addNode(reportcompletion19);

     WalkMachine *walkmachine = new WalkMachine("walkmachine");
     addNode(walkmachine);

     WaypointWalkMachine *waypointwalkmachine = new WaypointWalkMachine("waypointwalkmachine");
     addNode(waypointwalkmachine);

     SetVelocityMachine *setvelocitymachine = new SetVelocityMachine("setvelocitymachine");
     addNode(setvelocitymachine);

     GoToShapeMachine *gotoshapemachine = new GoToShapeMachine("gotoshapemachine");
     addNode(gotoshapemachine);

     PushObjectMachine *pushobjectmachine = new PushObjectMachine("pushobjectmachine");
     addNode(pushobjectmachine);

     VisualSearchMachine *visualsearchmachine = new VisualSearchMachine("visualsearchmachine");
     addNode(visualsearchmachine);

     startnode = dispatchDummyStart;

     localizemachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans40",reportSuccess,StateNode::successSignal));
     localizemachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans41",reportcompletion19,StateNode::failureSignal));
     waypointwalkmachine->addTransition(new CompletionTrans("completiontrans29",reportSuccess));
     waypointwalkmachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans42",reportFailure,StateNode::failureSignal));
     gotoshapemachine->addTransition(new CompletionTrans("completiontrans30",reportSuccess));
     gotoshapemachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans43",reportFailure,StateNode::failureSignal));
     pushobjectmachine->addTransition(new CompletionTrans("completiontrans31",reportSuccess));
     pushobjectmachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans44",reportFailure,StateNode::failureSignal));
     visualsearchmachine->addTransition(new CompletionTrans("completiontrans32",reportSuccess));
     visualsearchmachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans45",reportFailure,StateNode::failureSignal));

      startnode = NULL; // keeps the dummy node out of the Event Logger trace
      // Save these statenode pointers for use by dispatch,
      localizeMachine_ = localizemachine;
      walkMachine_ = walkmachine;
      waypointWalkMachine_ = waypointwalkmachine;
      setVelocityMachine_ = setvelocitymachine;
      goToShapeMachine_ = gotoshapemachine;
      pushObjectMachine_ = pushobjectmachine;
      visualSearchMachine_ = visualsearchmachine;
    };
   private:  // dummy copy and assignment to suppress compiler warnings
    Dispatch(const Dispatch&);
    Dispatch& operator=(const Dispatch&);
  };

  virtual void setup() {   // Pilot's setup
    SharedObject<WaypointWalkMC> waypointwalk_mc;
    SharedObject<WalkMC> walk_mc;
#ifdef PILOT_USES_SHARED_MOTION_COMMANDS
    walk_id = motman->addPersistentMotion(walk_mc, MotionManager::kIgnoredPriority);
    waypointwalk_id = motman->addPersistentMotion(waypointwalk_mc, MotionManager::kIgnoredPriority);
#endif
   StateNode *pilotWaitForDispatch = new StateNode("pilotWaitForDispatch");
   addNode(pilotWaitForDispatch);

   Dispatch *dispatch = new Dispatch("dispatch");
   addNode(dispatch);

   startnode = pilotWaitForDispatch;

   CompletionTrans *pilot_dispatch = new CompletionTrans("pilot_dispatch",pilotWaitForDispatch);
   dispatch->addTransition(pilot_dispatch);

    VRmixin::pilot->dispatch_ = dispatch;
  }

private:
  Pilot(const Pilot&); //!< copy constructor; do not call
  Pilot& operator=(const Pilot&); //!< assignment operator; do not call

  static PilotVerbosity_t verbosity;

  std::queue<PilotRequest*> requests;   //!< Queue of Pilot requests
  PilotRequest *curReq;
  unsigned int idCounter;   //!< Pilot request counter for assigning a unique id
  MotionManager::MC_ID walk_id;
  MotionManager::MC_ID waypointwalk_id;
  Dispatch *dispatch_;

  /*! @endcond */

  std::vector<ShapeRoot> defaultLandmarks;   // from $provide
  MapBuilderRequest* defaultLandmarkExtractor;   // from $provide
  Point initPos;   // from $provide
  AngTwoPi initHead;   // from $provide
  PilotRequest planReq;   // from $provide
  NavigationPlan plan;   // from $provide
  AngTwoPi maxTurn;   // from $provide
  bool pushingObj;   // from $provide
};

std::ostream& operator<<(std::ostream& os, const Pilot::NavigationStep &step);
std::ostream& operator<<(std::ostream& os, const Pilot::NavigationPlan &plan);

} // namespace

#endif