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

// Try using shared motion commands with Calliope
#  define PILOT_USES_SHARED_MOTION_COMMANDS


#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/PostureNode.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/RRT/GenericRRT.h"

#include "Vision/VisualOdometry/VisualOdometry.h"

#include "DualCoding/DualCoding.h"

using namespace std;

namespace DualCoding {

using namespace PilotTypes;

//! The Pilot is the top-level navigation engine for Tekkotsu.
/*! The Pilot accepts and executes PilotRequest instances.

  Navigation involves a combination of path planning, odometry, and
  landmark-based localization.  The Pilot uses the
  ShapeSpacePlannerXYTheta planner to find a path from the robot's
  current position to a target location that avoids collisions with
  worldShS shapes marked as obstacles.

  Once a path is found, the Pilot formulates a <em>navigation
  plan</em> consisting of a series of travel steps, turn steps, and
  localization steps.  The navigation planner is somewhat clever about
  where it places localization steps in order to avoid slowing down
  the robot's progress too much, but additional optimizations are
  possible.

  Finally, the Pilot executes the navigation plan.  Because both
  motion and odometry are subject to error, the robot can drift off
  course as it travels.  When this is detected via a localization
  step, the execution engine attempts a correction by inserting a new
  turn step in the navigation plan and altering the travel step that
  follows.
  
 */

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; }

  //! Functor used to sort gaze points by their relative bearing from the robot
  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 changeVelocity(float vx, float vy=0, float va=0);  //!< Change velocity for walk or setVelocity requests
  void cancelVelocity(); //!< forces completion of a setVelocity operation
  void setWorldBounds(float minX, float width, float minY, float height);
  void setWorldBounds(const Shape<PolygonData> &bounds); //!< Set world bounds to constrain the path planner
  void randomizeParticles(float widthIncr=1000, float heightIncr=1000); //!< Randomize the particles before localization
  void computeParticleBounds(float widthIncr=0, float heightIncr=0); //!< Compute bounds for random particle distribution based on world shapes

  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();
  };

  //! Compute optical flow for visual odometry.  Currently disabled.
  class RunOpticalFlow : public StateNode {
   public:
    RunOpticalFlow(const std::string &nodename="RunOpticalFlow") : 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::isWalkingFlag = false;
      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 StateNode {
   public:
    SetupLandmarkExtractor(const std::string &nodename="SetupLandmarkExtractor") : StateNode(nodename) {}
    virtual void doStart() {
      VRmixin::pilot->setupLandmarkExtractor();
    }
  };

  class PlanPath : public StateNode {
   public:
    PlanPath(const std::string &nodename="PlanPath") : 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;
      doPlan(plan, initPos, initHead, planReq);
    }
    void clearGraphics();
    void doPlan(NavigationPlan &plan, Point initPos, AngTwoPi initHead, PilotRequest &req);
    void showPath(const vector<RRTNodeXYTheta::NodeValue_t> &path);
  };

  //! Construct a navigation plan: mixture of traveling, turning, and localizing steps
  class ConstructNavPlan : public StateNode {
   public:
    ConstructNavPlan(const std::string &nodename="ConstructNavPlan") : 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;
      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 StateNode {
   public:
    ExecutePlan(const std::string &nodename="ExecutePlan") : StateNode(nodename), nextDisplacementInstruction() {}
    
    //! distance in mm we can be from a destination point and be "there"
    static const float allowableDistanceError;
    static const float allowableAngularError;

    //! Each turn or travel step in a navigation plan is a DisplacementInstruction
    struct DisplacementInstruction {
      DisplacementInstruction() : nextPoint(), angleToTurn(0), walkBackward(false) {};
      fmat::Column<2> nextPoint;
      AngSignTwoPi angleToTurn;
      bool walkBackward;
    };

    enum TurnAdjustType_t {
      localizationAdjust,
      collisionAdjust
    };

    class NextTask : public StateNode {
     public:
      NextTask(const std::string &nodename="NextTask") : StateNode(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
      }
    };

    //! Perform a travel step by walking forward or backward
    class Walk : public WalkNode {
     public:
      Walk(const std::string &nodename="Walk") : WalkNode(nodename) {}
      virtual void doStart() {
          PilotRequest &curReq = *VRmixin::pilot->curReq;
        DisplacementInstruction &nextDisplacementInstruction = getAncestor<ExecutePlan>()->nextDisplacementInstruction;
    
    #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
          float vx = (curReq.forwardSpeed > 0) ? curReq.forwardSpeed : 50.f;
    #else
          // these values are for Chiara
          float vx = (curReq.forwardSpeed > 0) ? curReq.forwardSpeed : 15.f;
    #endif
          float dist = nextDisplacementInstruction.nextPoint.norm();
          if ( nextDisplacementInstruction.walkBackward )
            dist *= -1;
          getMC()->setTargetDisplacement(dist,0,0,vx,0,0);
          motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
      }
    };

    //! Perform a turn step
    class Turn : public WalkNode {
     public:
      Turn(const std::string &nodename="Turn") : WalkNode(nodename) {}
      virtual void doStart() {
          PilotRequest &curReq = *VRmixin::pilot->curReq;
        DisplacementInstruction &nextDisplacementInstruction = getAncestor<ExecutePlan>()->nextDisplacementInstruction;
    
          // std::cout<<"turning \n";
    
    #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
          float va = (curReq.turnSpeed > 0) ? curReq.turnSpeed : 0.25f;
    #else
          // these values are for Chiara
          float va = (curReq.turnSpeed > 0) ? curReq.turnSpeed : 0.1f;
    #endif
          float turnAngle = nextDisplacementInstruction.angleToTurn;
          getMC()->setTargetDisplacement(0,0,turnAngle,0,0,va);
          motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
      }
    };

    //! Perform an adjusting turn if we localized successfully, or a collision occurred
    class AdjustHeading : public WalkNode {
     public:
      AdjustHeading(const std::string &nodename, const ExecutePlan::TurnAdjustType_t _adjustmentType) : WalkNode(nodename), adjustmentType(_adjustmentType) {}
      const ExecutePlan::TurnAdjustType_t adjustmentType;  // cache the constructor's parameter
        virtual void doStart() {
      NavigationPlan &plan = getAncestor<Pilot>()->plan;
          doAdjust(plan);
        }
        virtual void doAdjust(NavigationPlan &plan);
    };

    class ExecuteStep : public StateNode {
     public:
      ExecuteStep(const std::string &nodename="ExecuteStep") : StateNode(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);
    };

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

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

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

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

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

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

     Turn *pretravel = new Turn("pretravel");
     addNode(pretravel);
     pretravel->setMC(VRmixin::pilot->walk_id);

     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,headingStep));
     execute->addTransition(new SignalTrans<NavigationStepType_t>("signaltrans5",pretravel,preTravelStep));
     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",next,StateNode::failureSignal));
     adjust->addTransition(new CompletionTrans("completiontrans3",next));
     walker->addTransition(new CompletionTrans("completiontrans4",next));
     turner->addTransition(new CompletionTrans("completiontrans5",next));
     pretravel->addTransition(new CompletionTrans("completiontrans6",execute));
     next->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans8",execute,StateNode::successSignal));
     next->addTransition(new CompletionTrans("completiontrans7",postmachinecompletion2));

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

    }
    DisplacementInstruction nextDisplacementInstruction;   // from $provide
  };
  
  //================ LocalizationUtility ================

  //! State machine called by various Pilot functions to perform a landmark search and localization.
  class LocalizationUtility : public StateNode {
   public:
    LocalizationUtility(const std::string &nodename="LocalizationUtility") : StateNode(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 StateNode {
     public:
      CheckEnoughLandmarks(const std::string &nodename="CheckEnoughLandmarks") : StateNode(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(VRmixin::localShS, gazePoints.front()));
          //std::cout << "Pilot localizer moving to gazepoint " << gazePoints.front() << std::endl;
          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 WalkNode {
     public:
      PrepareForNextGazePoint(const std::string &nodename="PrepareForNextGazePoint") : WalkNode(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 WalkNode {
     public:
      ReturnToInitialHeading(const std::string &nodename="ReturnToInitialHeading") : WalkNode(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;
    }
    getMC()->setTargetDisplacement(0,0,turnAmount,0);
    motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
        }
    };

#endif // TGT_HAS_HEAD

    class Localize : public StateNode {
     public:
      Localize(const std::string &nodename="Localize") : StateNode(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>("signaltrans9",outofoptions,StateNode::failureSignal));
     take->addTransition(new EventTrans("eventtrans1",checkEnough,EventBase::mapbuilderEGID,(size_t)take,EventBase::statusETID));
     checkEnough->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans10",loc,StateNode::successSignal));
     checkEnough->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans11",choosegazepoints1,StateNode::failureSignal));
     choosegazepoints1->addTransition(new NullTrans("nulltrans1",loop));
     loop->addTransition(new CompletionTrans("completiontrans8",statenode1));
     statenode1->addTransition(new TimeOutTrans("timeouttrans1",takenextpic,1000));
     loop->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans12",loop,StateNode::successSignal));
     loop->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans13",outofoptions,StateNode::failureSignal));
     takenextpic->addTransition(new EventTrans("eventtrans2",checkEnough2,EventBase::mapbuilderEGID,(size_t)takenextpic,EventBase::statusETID));
     checkEnough2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans14",returntoinitialheading1,StateNode::successSignal));
     returntoinitialheading1->addTransition(new CompletionTrans("completiontrans9",loc));
     checkEnough2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans15",loop,StateNode::failureSignal));
     loc->addTransition(new NullTrans("nulltrans2",postmachinesuccess1));
     postmachinesuccess1->addTransition(new NullTrans("nulltrans3",postmachinecompletion3));
     outofoptions->addTransition(new CompletionTrans("completiontrans10",postmachinefailure1));
     postmachinefailure1->addTransition(new NullTrans("nulltrans4",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("nulltrans5",loc));
     loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans16",postmachinesuccess2,StateNode::successSignal));
     loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans17",postmachinefailure2,StateNode::failureSignal));
    }
  };

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

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

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

    class SetWalking : public StateNode {
     public:
      SetWalking(const std::string &nodename="SetWalking") : StateNode(nodename) {}
      virtual void doStart() {
          VRmixin::isWalkingFlag = true;
          // cout << "Turning On Walk" << endl;
      }
    };

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

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

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

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

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

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

     walker->addTransition(new CompletionTrans("completiontrans11",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();
    };

    class SetWalking : public StateNode {
     public:
      SetWalking(const std::string &nodename="SetWalking") : StateNode(nodename) {}
      virtual void doStart() {
          VRmixin::isWalkingFlag = true;
          // cout << "Turning On Walk" << endl;
      }
    };

    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);

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

     walker->addTransition(new CompletionTrans("completiontrans12",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);

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

     walker->addTransition(new CompletionTrans("completiontrans13",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 StateNode {
   public:
    GoToShapeMachine(const std::string &nodename="GoToShapeMachine") : StateNode(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 = VRmixin::theAgent->getCentroid();
            initHead = VRmixin::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);

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

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

     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("nulltrans9",check));
     check->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans18",reportcompletion4,StateNode::failureSignal));
     check->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans19",setuplandmarkextractor2,StateNode::successSignal));
     setuplandmarkextractor2->addTransition(new NullTrans("nulltrans10",setupplan));
     loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans20",setupplan,StateNode::successSignal));
     loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans21",setupplan,StateNode::failureSignal));
     setupplan->addTransition(new NullTrans("nulltrans11",setupplannode1));
     setupplannode1->addTransition(new NullTrans("nulltrans12",planpath));
     planpath->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans22",reportcompletion5,GenericRRTBase::START_COLLIDES));
     planpath->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans23",reportcompletion6,GenericRRTBase::END_COLLIDES));
     planpath->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans24",reportcompletion7,GenericRRTBase::MAX_ITER));
     planpath->addTransition(new CompletionTrans("completiontrans14",constructnavplan));
     planpath->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans25",reportcompletion8,StateNode::failureSignal));

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

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

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

    }
  };
  
  class ClearOldPath : public StateNode {
   public:
    ClearOldPath(const std::string &nodename="ClearOldPath") : StateNode(nodename) {}
    virtual void doStart() {
      GET_SHAPE(plannedInitToObjPath, GraphicsData, VRmixin::worldShS);
      plannedInitToObjPath.deleteShape();
      GET_SHAPE(plannedObjToDestPath, GraphicsData, VRmixin::worldShS);
      plannedObjToDestPath.deleteShape();
      GET_SHAPE(plannedPath, GraphicsData, VRmixin::worldShS);
      plannedPath.deleteShape();
    }
  };
  
  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;
        
      MapBuilderRequest *m = new MapBuilderRequest(MapBuilderRequest::localMap);
      std::vector<Point> ptVec;
      Point pt1 = Point(100, 100, 0, allocentric);
      Point pt2 = Point(100, -100, 0, allocentric);
      Point pt3 = Point(-100, -100, 0, allocentric);
      Point pt4 = Point(-100, 100, 0, allocentric);
      Point objPt = VRmixin::pilot->curReq->objectShape->getCentroid();
      ptVec.push_back(objPt);
      ptVec.push_back(objPt+pt1);
      ptVec.push_back(objPt+pt2);
      ptVec.push_back(objPt+pt3);
      ptVec.push_back(objPt+pt4);
      ptVec.push_back(objPt);
      NEW_SHAPE_N(polygon, PolygonData, new PolygonData(VRmixin::worldShS, ptVec, false));
      polygon->setObstacle(false);
      m->searchArea = polygon;
      m->addAttributes(VRmixin::pilot->curReq->objectShape);
      mapreq = *m;
    }
  };
  
  class LookForTarget : public MapBuilderNode {
   public:
    LookForTarget(const std::string &nodename="LookForTarget") : MapBuilderNode(nodename) {}
    virtual void doStart() {
      //MapBuilderRequest *m = VRmixin::pilot->curReq->objectExtractor;
      //if ( m == NULL )
      //  cancelThisRequest();
      //else
      //  mapreq = *m;
        
      MapBuilderRequest *m = new MapBuilderRequest(MapBuilderRequest::localMap);
      std::vector<Point> ptVec;
      Point pt1 = Point(100, 100, 0, allocentric);
      Point pt2 = Point(100, -100, 0, allocentric);
      Point pt3 = Point(-100, -100, 0, allocentric);
      Point pt4 = Point(-100, 100, 0, allocentric);
      Point ctPt = VRmixin::pilot->curReq->targetShape->getCentroid();
      ptVec.push_back(ctPt);
      ptVec.push_back(ctPt+pt1);
      ptVec.push_back(ctPt+pt2);
      ptVec.push_back(ctPt+pt3);
      ptVec.push_back(ctPt+pt4);
      ptVec.push_back(ctPt);
      NEW_SHAPE_N(polygon, PolygonData, new PolygonData(VRmixin::worldShS, ptVec, false));
      polygon->setObstacle(false);
      m->searchArea = polygon;
      m->addAttributes(VRmixin::pilot->curReq->objectShape);
      mapreq = *m;
    }
  };

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

  class PushObjectMachine : public StateNode {
   public:
    PushObjectMachine(const std::string &nodename="PushObjectMachine") : StateNode(nodename), initToObjPlan(), objToDestPlan(), startPos(), objPos(), endPos(), startOri(), acquireOri(), backupOri(), backupPt(), acquirePt(), useSimpleWayObjToDest(), useSimpleWayInitToObj(), isObs() {}
  
    //static const int backupDist = 600;
    //static const int obstDist = 150;
    //static const int robotDiam = 150;
    //static const int objSize = 50;
    static int backupDist;
    static int obstDist;
    static int robotDiam;
    static int objSize;
    
    static float prePushTurnSpeed;
    static float prePushForwardSpeed;
    static float pushTurnSpeed;
    static float pushForwardSpeed;

    
    
    
    


    static Point FindFreeSpace(ShapeRoot &targetShape, int targetDistance, int obstacleDistance, AngTwoPi orientation) {
      float diagonalLength = sqrt(VRmixin::worldSkS.getWidth()*VRmixin::worldSkS.getWidth() + 
          VRmixin::worldSkS.getHeight()*VRmixin::worldSkS.getHeight());
      float scale = diagonalLength / (obstacleDistance * 10);
      float setx = (VRmixin::worldSkS.getHeight()/2) - targetShape->getCentroid().coordX() * scale;
      float sety = (VRmixin::worldSkS.getWidth()/2) - targetShape->getCentroid().coordY() * scale;
      VRmixin::worldSkS.setTmat(scale, setx, sety);
      bool isObs = targetShape->isObstacle();
      targetShape->setObstacle(false);
      
      NEW_SKETCH_N(obstacles, bool, visops::zeros(VRmixin::worldSkS));
      NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(VRmixin::worldShS));
      SHAPEVEC_ITERATE(ellipses, EllipseData, ellipse) {
        if (ellipse->isObstacle()) {
          NEW_SKETCH_N(s1, bool, ellipse->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(VRmixin::worldShS));
      SHAPEVEC_ITERATE(blobs, BlobData, blob) {
        if (blob->isObstacle()) {
          NEW_SKETCH_N(s1, bool, blob->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      NEW_SHAPEVEC(lines, LineData, select_type<LineData>(VRmixin::worldShS));
      SHAPEVEC_ITERATE(lines, LineData, line) {
        if (line->isObstacle()) {
          NEW_SKETCH_N(s1, bool, line->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      
      NEW_SKETCH_N(dist, float, visops::edist(obstacles));
      NEW_SKETCH_N(cand, bool, dist > obstacleDistance*scale);
      //NEW_SKETCH_N(cand, bool, (dist > distance*scale) & (dist < (distance+10)*scale));
      
      //NEW_SHAPE(point, PointData, new PointData(VRmixin::worldShS, targetPoint));
      //NEW_SKETCH_N(target, bool, point->getRendering());
      NEW_SKETCH_N(target, bool, targetShape->getRendering());
      NEW_SKETCH_N(tdist, float, visops::ebdist(target, !cand, 2*diagonalLength));
      NEW_SKETCH_N(cdist, bool, (tdist >= targetDistance*scale) & (tdist <= targetDistance*scale+1));
      //NEW_SKETCH_N(cdist, bool, tdist2 & cand);
      
      AngTwoPi ori = orientation + AngTwoPi(M_PI);
      //float length = sqrt(VRmixin::worldSkS.getWidth()*VRmixin::worldSkS.getWidth() + VRmixin::worldSkS.getHeight()*VRmixin::worldSkS.getHeight()) / VRmixin::worldSkS.getScaleFactor();
      float length = diagonalLength / scale;
      Point bp(targetShape->getCentroid().coordX()+length*std::cos(ori), targetShape->getCentroid().coordY()+length*std::sin(ori), 0, allocentric);
      NEW_SHAPE(line, LineData, new LineData(VRmixin::worldShS, targetShape->getCentroid(), bp));
      line->setColor("red");
      NEW_SKETCH_N(ls, bool, line->getRendering());
      NEW_SKETCH_N(lsdist, float, visops::edist(ls));
      NEW_SKETCH_N(ldist, float, visops::edist(ls) * cdist);
      line.deleteShape();
      
      int pos = ldist->findMinPlus();
      if (pos==-1) return Point(0,0,0,unspecified);
      NEW_SKETCH_N(final, bool, visops::zeros(ldist));
      final[pos] = true;
      final->setColor("green");
      targetShape->setObstacle(isObs);
      Point pt = final->indexPoint(pos);
      std::cout << "Found free space at point: " << pt << std::endl;
      return pt;
    }
    
    static Point FindFreeSpace2(ShapeRoot &targetShape, int targetDistance, int obstacleDistance, AngTwoPi orientation) {
      float diagonalLength = sqrt(VRmixin::worldSkS.getWidth()*VRmixin::worldSkS.getWidth() + VRmixin::worldSkS.getHeight()*VRmixin::worldSkS.getHeight());
      float scale = diagonalLength / (obstacleDistance * 10);
      float setx = (VRmixin::worldSkS.getHeight()/2) - targetShape->getCentroid().coordX() * scale;
      float sety = (VRmixin::worldSkS.getWidth()/2) - targetShape->getCentroid().coordY() * scale;
      VRmixin::worldSkS.setTmat(scale, setx, sety);
      bool isObs = targetShape->isObstacle();
      targetShape->setObstacle(false);
      
      NEW_SKETCH(obstacles, bool, visops::zeros(VRmixin::worldSkS));
      NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(VRmixin::worldShS));
      SHAPEVEC_ITERATE(ellipses, EllipseData, ellipse) {
        if (ellipse->isObstacle()) {
          NEW_SKETCH(s1, bool, ellipse->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(VRmixin::worldShS));
      SHAPEVEC_ITERATE(blobs, BlobData, blob) {
        if (blob->isObstacle()) {
          NEW_SKETCH(s1, bool, blob->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      NEW_SHAPEVEC(lines, LineData, select_type<LineData>(VRmixin::worldShS));
      SHAPEVEC_ITERATE(lines, LineData, line) {
        if (line->isObstacle()) {
          NEW_SKETCH(s1, bool, line->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      
      NEW_SKETCH(dist, float, visops::edist(obstacles));
      NEW_SKETCH(cand, bool, dist > obstacleDistance*scale);
      //NEW_SKETCH(cand, bool, (dist > distance*scale) & (dist < (distance+10)*scale));
      
      //NEW_SHAPE(point, PointData, new PointData(VRmixin::worldShS, targetPoint));
      //NEW_SKETCH(target, bool, point->getRendering());
      NEW_SKETCH(target, bool, targetShape->getRendering());
      NEW_SKETCH(tdist, float, visops::ebdist(target, !cand, 2*diagonalLength));
      NEW_SKETCH(cdist, bool, (tdist >= targetDistance*scale) & (tdist <= targetDistance*scale+1));
      //NEW_SKETCH(cdist, bool, tdist2 & cand);
      
      AngTwoPi ori = orientation + AngTwoPi(M_PI);
      //float length = sqrt(VRmixin::worldSkS.getWidth()*VRmixin::worldSkS.getWidth() + VRmixin::worldSkS.getHeight()*VRmixin::worldSkS.getHeight()) / VRmixin::worldSkS.getScaleFactor();
      float length = diagonalLength / scale;
      Point bp(targetShape->getCentroid().coordX()+length*std::cos(ori), targetShape->getCentroid().coordY()+length*std::sin(ori), 0, allocentric);
      NEW_SHAPE(line, LineData, new LineData(VRmixin::worldShS, targetShape->getCentroid(), bp));
      line->setColor("red");
      NEW_SKETCH(ls, bool, line->getRendering());
      NEW_SKETCH(lsdist, float, visops::edist(ls));
      NEW_SKETCH(ldist, float, visops::edist(ls) * cdist);
      line.deleteShape();
      
      int pos = ldist->findMinPlus();
      if (pos==-1) return Point(0,0,0,unspecified);
      NEW_SKETCH(final, bool, visops::zeros(ldist));
      final[pos] = true;
      final->setColor("green");
      targetShape->setObstacle(isObs);
      Point pt = final->indexPoint(pos);
      std::cout << "Found free space at point: " << pt << std::endl;
      return pt;
    }
  
    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;
        Point &startPos = getAncestor<PushObjectMachine>()->startPos;
        Point &objPos = getAncestor<PushObjectMachine>()->objPos;
        Point &endPos = getAncestor<PushObjectMachine>()->endPos;
        AngTwoPi &startOri = getAncestor<PushObjectMachine>()->startOri;
        bool &isObs = getAncestor<PushObjectMachine>()->isObs;
          
        //std::cout << "--------SetUpObjToDest" << std::endl;
          startPos = VRmixin::theAgent->getCentroid();
          objPos = VRmixin::pilot->curReq->objectShape->getCentroid();
          endPos = VRmixin::pilot->curReq->targetShape->getCentroid();
          startOri = VRmixin::theAgent->getOrientation();
          
          plan.clear();
          initPos = objPos;
          planReq = *VRmixin::pilot->curReq;
          isObs = planReq.objectShape->isObstacle();
          planReq.objectShape->setObstacle(false);
          pushingObj = false;
          planReq.displayPath = false;
      }
    };

    class AddBackupPt : public StateNode {
     public:
      AddBackupPt(const std::string &nodename="AddBackupPt") : StateNode(nodename) {}
      virtual void doStart() {
          /*  NEEDS TO BE UPDATED
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
        Point &endPos = getAncestor<PushObjectMachine>()->endPos;
        Point &backupPt = getAncestor<PushObjectMachine>()->backupPt;
        AngTwoPi &backupOri = getAncestor<PushObjectMachine>()->backupOri;
          
          //std::cout << "--------AddBackupPt" << std::endl;
          size_t s = plan.path.size();
          Point final = plan.path[s-1];
          Point prev = plan.path[s-2];
          Point vec = final-prev;
          AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
          Point temp = VRmixin::pilot->curReq->objectShape->getCentroid();
          VRmixin::pilot->curReq->objectShape->setPosition(final);
          //std::cout << "tempBackupOri: " << ori << std::endl;
          backupPt = FindFreeSpace(VRmixin::pilot->curReq->objectShape, backupDist, obstDist, ori);
          VRmixin::pilot->curReq->objectShape->setPosition(temp);
          ReferenceFrameType_t t = backupPt.getRefFrameType();
          if (t == unspecified) {
            std::cout << "Not enough space at destination." << std::endl;
            postStateFailure();
            return;
          }
          Point vec2 = endPos - backupPt;
          backupOri = AngTwoPi(std::atan2(vec2.coordY(), vec2.coordX()));
          //std::cout << "backupPt: " << backupPt << ", backupOri: " << backupOri << std::endl;
          */
          postStateSuccess();
      }
    };

    class AddAcquirePt : public StateNode {
     public:
      AddAcquirePt(const std::string &nodename="AddAcquirePt") : StateNode(nodename) {}
      virtual void doStart() {
          /* NEEDS TO BE UPDATED
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
        Point &objPos = getAncestor<PushObjectMachine>()->objPos;
        Point &acquirePt = getAncestor<PushObjectMachine>()->acquirePt;
        AngTwoPi &acquireOri = getAncestor<PushObjectMachine>()->acquireOri;
          
          //std::cout << "--------AddAcquirePt" << std::endl;
          Point first = plan.path[0];
          Point next = plan.path[1];
          Point vec = next-first;
          AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
          //std::cout << "tempAcquireOri: " << ori << std::endl;
          acquirePt = FindFreeSpace(VRmixin::pilot->curReq->objectShape, backupDist, obstDist, ori);
          ReferenceFrameType_t t = acquirePt.getRefFrameType();
          if (t == unspecified) {
            std::cout << "Not enough space for acquiring object." << std::endl;
            postStateFailure();
            return;
          }
          Point vec2 = objPos - acquirePt;
          acquireOri = AngTwoPi(std::atan2(vec2.coordY(), vec2.coordX()));
          //std::cout << "acquirePt: " << acquirePt << ", acquireOri: " << acquireOri << std::endl;
          */
          postStateSuccess();
      }
    };
    
    class SetUpObjToDest2 : public StateNode {
     public:
      SetUpObjToDest2(const std::string &nodename="SetUpObjToDest2") : StateNode(nodename) {}
      virtual void doStart() {
        Point &initPos = getAncestor<Pilot>()->initPos;
        AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
        PilotRequest &planReq = getAncestor<Pilot>()->planReq;
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
        bool &pushingObj = getAncestor<Pilot>()->pushingObj;
        Point &backupPt = getAncestor<PushObjectMachine>()->backupPt;
        Point &objPos = getAncestor<PushObjectMachine>()->objPos;
        AngTwoPi &acquireOri = getAncestor<PushObjectMachine>()->acquireOri;
        AngTwoPi &backupOri = getAncestor<PushObjectMachine>()->backupOri;
          
        //std::cout << "--------SetUpObjToDest2" << std::endl;
          plan.clear();
          
          initPos = objPos;
          initHead = acquireOri;
          planReq = *VRmixin::pilot->curReq;
          planReq.objectShape->setObstacle(false);
          pushingObj = true;
          NEW_SHAPE(backup, PointData, new PointData(VRmixin::worldShS, backupPt));
          backup->setObstacle(false);
          planReq.targetShape = backup;
          planReq.targetHeading = backupOri;
          planReq.displayPath = false;
      }
    };
    
    class SetUpInitToObj : public StateNode {
     public:
      SetUpInitToObj(const std::string &nodename="SetUpInitToObj") : StateNode(nodename) {}
      virtual void doStart() {
        Point &initPos = getAncestor<Pilot>()->initPos;
        AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
        PilotRequest &planReq = getAncestor<Pilot>()->planReq;
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
        bool &pushingObj = getAncestor<Pilot>()->pushingObj;
        Point &startPos = getAncestor<PushObjectMachine>()->startPos;
        AngTwoPi &startOri = getAncestor<PushObjectMachine>()->startOri;
        Point &acquirePt = getAncestor<PushObjectMachine>()->acquirePt;
        AngTwoPi &acquireOri = getAncestor<PushObjectMachine>()->acquireOri;
          
        //std::cout << "--------SetUpInitToObj" << std::endl;
          plan.clear();
          
          initPos = startPos;
          initHead = startOri;
          planReq = *VRmixin::pilot->curReq;
          planReq.objectShape->setObstacle(true);
          pushingObj = false;
          NEW_SHAPE(acquire, PointData, new PointData(VRmixin::worldShS, acquirePt));
          acquire->setObstacle(false);
          planReq.targetShape = acquire;
          planReq.targetHeading = acquireOri;
          planReq.displayPath = false;
          planReq.turnSpeed = prePushTurnSpeed;
          planReq.forwardSpeed = prePushForwardSpeed;
      }
    };
    
    class ChoosePathInitToObj : public StateNode {
     public:
      ChoosePathInitToObj(const std::string &nodename="ChoosePathInitToObj") : StateNode(nodename) {}
      virtual void doStart() {
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
        Point &startPos = getAncestor<PushObjectMachine>()->startPos;
        Point &objPos = getAncestor<PushObjectMachine>()->objPos;
        AngTwoPi &acquireOri = getAncestor<PushObjectMachine>()->acquireOri;
        NavigationPlan &objToDestPlan = getAncestor<PushObjectMachine>()->objToDestPlan;
          
        //std::cout << "--------ChoosePathInitToObj" << std::endl;
          objToDestPlan.clear();
          objToDestPlan.path = plan.path;
          
          Point vec = objPos-startPos;
          AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
          float d = vec.xyNorm();
          float oriDiff = std::abs(AngSignPi(ori-acquireOri));
          //std::cout << "d: " << d << ", oriDiff:" << oriDiff << ", ori: " << ori << ", acquireOri: " << acquireOri << std::endl;
          if (d < backupDist*1.2 && oriDiff < M_PI/180*45) {
            //FetchObj
            std::cout << "Use simple way (straight line) from init to obj" << std::endl;
            postStateSuccess();
          }
          else {
            //plan path
            std::cout << "Use complex way (need path planning) from init to obj" << std::endl;
            postStateFailure();
          }
      }
    };
    
    /*$nodeclass ChoosePathObjToDest : StateNode : doStart {
      //$reference PushObjectMachine::endPos, PushObjectMachine::backupOri;
      
    std::cout << "--------ChoosePathObjToDest" << std::endl;
      Point curPos = VRmixin::theAgent->getCentroid();
      //AngTwoPi curHead = VRmixin::theAgent->getOrientation();
      Point vec = endPos-curPos;
      AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
      float d = vec.xyNorm();
      float oriDiff = std::abs(AngSignPi(ori-backupOri));
      std::cout << "d: " << d << ", oriDiff:" << oriDiff << ", ori: " << ori << ", backupOri: " << backupOri << std::endl;
      if (d < backupDist*1.05 && oriDiff < M_PI/180*30) {
        //AdjustPush
        std::cout << "Use simple way (straight line) from obj to dest" << std::endl;
        postStateSuccess();
      }
      else {
        //plan path
        std::cout << "Use complex way (need path planning) from obj to dest" << std::endl;
        postStateFailure();
      }
    }*/
    
    class ChoosePathObjToDest : public StateNode {
     public:
      ChoosePathObjToDest(const std::string &nodename="ChoosePathObjToDest") : StateNode(nodename) {}
      virtual void doStart() {
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
        Point &endPos = getAncestor<PushObjectMachine>()->endPos;
        Point &backupPt = getAncestor<PushObjectMachine>()->backupPt;
        AngTwoPi &backupOri = getAncestor<PushObjectMachine>()->backupOri;
        NavigationPlan &initToObjPlan = getAncestor<PushObjectMachine>()->initToObjPlan;
          
        //std::cout << "--------ChoosePathObjToDest" << std::endl;
          initToObjPlan.clear();
          initToObjPlan.path = plan.path;
          initToObjPlan.steps = plan.steps;
          
          Point curPos = VRmixin::theAgent->getCentroid();
          //AngTwoPi curHead = VRmixin::theAgent->getOrientation();
          Point vec = endPos-curPos;
          AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
          Point temp = VRmixin::pilot->curReq->objectShape->getCentroid();
          VRmixin::pilot->curReq->objectShape->setPosition(endPos);
          //std::cout << "tempBackupOri: " << ori << std::endl;
          backupPt = FindFreeSpace(VRmixin::pilot->curReq->objectShape, backupDist, obstDist, ori);
          VRmixin::pilot->curReq->objectShape->setPosition(temp);
          ReferenceFrameType_t t = backupPt.getRefFrameType();
          if (t == unspecified) {
            std::cout << "Not enough space at destination." << std::endl;
            //postStateFailure();
            return;
          }
          Point vec2 = endPos - backupPt;
          backupOri = AngTwoPi(std::atan2(vec2.coordY(), vec2.coordX()));
          //std::cout << "backupPt: " << backupPt << ", backupOri: " << backupOri << std::endl;
          float d = vec.xyNorm();
          float oriDiff = std::abs(AngSignPi(ori-backupOri));
          //std::cout << "d: " << d << ", oriDiff:" << oriDiff << ", ori: " << ori << ", backupOri: " << backupOri << std::endl;
          if (d < backupDist*1.2 && oriDiff < M_PI/180*45) {
            //AdjustPush
            std::cout << "Use simple way (straight line) from obj to dest" << std::endl;
            postStateSuccess();
          }
          else {
            //plan path
            std::cout << "Use complex way (need path planning) from obj to dest" << std::endl;
            postStateFailure();
          }
      }
    };
    
    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;
    
        //std::cout << "--------SetUpExecute2" << std::endl;
          plan.clear();
          plan.path = objToDestPlan.path;
    
          initPos = VRmixin::theAgent->getCentroid();
          initHead = VRmixin::theAgent->getOrientation();
          PilotRequest &req = *VRmixin::pilot->curReq;
          req.turnSpeed = pushTurnSpeed;
          req.forwardSpeed = pushForwardSpeed;
          planReq = req;
          maxTurn = 0.f;
          pushingObj = true;
          req.collisionAction = PilotTypes::collisionIgnore;
          planReq.displayPath = false;
      }
    };
    
    class DisplayStraightLineInitToObj : public StateNode {
     public:
      DisplayStraightLineInitToObj(const std::string &nodename="DisplayStraightLineInitToObj") : StateNode(nodename) {}
      virtual void doStart() {
        Point &objPos = getAncestor<PushObjectMachine>()->objPos;
        Point &startPos = getAncestor<PushObjectMachine>()->startPos;
          
        //std::cout << "--------DisplayStraightLineInitToObj" << std::endl;
          // Display path (straight line) from init to obj
          NEW_SHAPE(plannedInitToObjPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
          GraphicsData::xyPair U, V;
          U.first = startPos.coordX(); U.second = startPos.coordY();
          V.first = objPos.coordX(); V.second = objPos.coordY();
          plannedInitToObjPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
          //NEW_SHAPE(plannedInitToObjPath2, LineData, new LineData(VRmixin::worldShS, startPos, objPos));
          //plannedInitToObjPath2->setColor("red");
      }
    };
    
    class DisplayStraightLineObjToDest : public StateNode {
     public:
      DisplayStraightLineObjToDest(const std::string &nodename="DisplayStraightLineObjToDest") : StateNode(nodename) {}
      virtual void doStart() {
        Point &objPos = getAncestor<PushObjectMachine>()->objPos;
        Point &endPos = getAncestor<PushObjectMachine>()->endPos;
          
        //std::cout << "--------DisplayStraightLineObjToDest" << std::endl;
          // Display path (straight line) from obj to dest
          NEW_SHAPE(plannedObjToDestPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
          GraphicsData::xyPair U, V;
          U.first = objPos.coordX(); U.second = objPos.coordY();
          V.first = endPos.coordX(); V.second = endPos.coordY();
          plannedObjToDestPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
          //NEW_SHAPE(plannedObjToDestPath2, LineData, new LineData(VRmixin::worldShS, objPos, endPos));
          //plannedObjToDestPath2->setColor("red");
      }
    };
    
    class DisplayPathInitToObj : public StateNode {
     public:
      DisplayPathInitToObj(const std::string &nodename="DisplayPathInitToObj") : StateNode(nodename) {}
      virtual void doStart() {
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
        Point &objPos = getAncestor<PushObjectMachine>()->objPos;
          
          //std::cout << "--------DisplayPathInitToObj" << std::endl;
          // Display path from init to obj
          NEW_SHAPE(plannedInitToObjPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
          GraphicsData::xyPair U, V;
          for ( size_t i = 1; i < plan.path.size(); i++ ) {
      U.first = plan.path[i-1].x; U.second = plan.path[i-1].y;
      V.first = plan.path[i].x; V.second = plan.path[i].y;
      plannedInitToObjPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
          }
          U.first = plan.path.back().x; U.second = plan.path.back().y;
          V.first = objPos.coordX(); V.second = objPos.coordY();
          plannedInitToObjPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
      }
    };
    
    class DisplayPathObjToDest : public StateNode {
     public:
      DisplayPathObjToDest(const std::string &nodename="DisplayPathObjToDest") : StateNode(nodename) {}
      virtual void doStart() {
        NavigationPlan &plan = getAncestor<Pilot>()->plan;
        Point &endPos = getAncestor<PushObjectMachine>()->endPos;
          
        //std::cout << "--------DisplayPathObjToDest" << std::endl;
          // Display path from obj to dest
          NEW_SHAPE(plannedObjToDestPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
          GraphicsData::xyPair U, V;
          for ( size_t i = 1; i < plan.path.size(); i++ ) {
      U.first = plan.path[i-1].x; U.second = plan.path[i-1].y;
      V.first = plan.path[i].x; V.second = plan.path[i].y;
      plannedObjToDestPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
          }
          U.first = plan.path.back().x; U.second = plan.path.back().y;
          V.first = endPos.coordX(); V.second = endPos.coordY();
          plannedObjToDestPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
      }
    };

    class WalkBackward : public WalkNode {
     public:
      WalkBackward(const std::string &nodename="WalkBackward") : WalkNode(nodename) {}
      virtual void doStart() {
        //std::cout << "--------WalkBackward" << std::endl;
          MMAccessor<WalkMC> walk_acc(getMC());
          getMC()->setTargetDisplacement(-backupDist+robotDiam,0,0,0);
          motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
      }
    };
    
    class WalkForward : public WalkNode {
     public:
      WalkForward(const std::string &nodename="WalkForward") : WalkNode(nodename) {}
      virtual void doStart() {
        //std::cout << "--------WalkForward" << std::endl;
          getMC()->setTargetDisplacement(backupDist-robotDiam,0,0,0);
          motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);        
      }
    };
    
    class FetchObj : public WalkNode {
     public:
      FetchObj(const std::string &nodename="FetchObj") : WalkNode(nodename) {}
      virtual void doStart() {
        Point &objPos = getAncestor<PushObjectMachine>()->objPos;
          
          std::cout << "--------Fetch the object" << std::endl;
          std::cout << "curPos: " << VRmixin::theAgent->getCentroid() << ", curHead: "
        << VRmixin::theAgent->getOrientation() << std::endl;
          PilotRequest &preq = *VRmixin::pilot->curReq;
          preq.collisionAction = PilotTypes::collisionIgnore;
          
          AngSignPi turnAng;
          float dist;
          if ( preq.objectMatcher == NULL ) {
            //std::cout << "No objectMatcher specified" << std::endl;
      Point curPos = VRmixin::theAgent->getCentroid();
            AngTwoPi curHead = VRmixin::theAgent->getOrientation();
            Point vec = objPos - curPos;
            float x = vec.coordX(), y = vec.coordY();
            turnAng = AngSignPi(std::atan2(y, x)) - curHead;
            dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
          } else {
      ShapeRoot localObj = (*preq.objectMatcher)(preq.objectShape);
      if ( ! localObj.isValid() ) {
        std::cout << "Can't find the object" << std::endl;
        Point curPos = VRmixin::theAgent->getCentroid();
              AngTwoPi curHead = VRmixin::theAgent->getOrientation();
              Point vec = objPos - curPos;
              float x = vec.coordX(), y = vec.coordY();
              turnAng = AngSignPi(std::atan2(y, x)) - curHead;
              dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
        //postStateFailure();
        //return;
      } else {
        Point ctPt = localObj->getCentroid();
              float x = ctPt.coordX(), y = ctPt.coordY();
              turnAng = AngSignPi(std::atan2(y, x));
              dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
            }
          }
          
          std::cout << "Turn " << float(turnAng)/M_PI*180.f << " degree" << std::endl;
          getMC()->setTargetDisplacement(0, 0, turnAng, 0);
          std::cout << "Walk forward " << dist << " mm" << std::endl;
          // wp_acc->addEgocentricWaypoint(dist, 0, 0, true, pushForwardSpeed, 0.f);
          motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
      }
    };
    
    class AdjustPush : public WaypointWalkNode {
     public:
      AdjustPush(const std::string &nodename="AdjustPush") : WaypointWalkNode(nodename) {}
      virtual void doStart() {
        Point &endPos = getAncestor<PushObjectMachine>()->endPos;
          
          std::cout << "--------Adjust before pushing finish" << std::endl;
          std::cout << "curPos: " << VRmixin::theAgent->getCentroid() << ", curHead: "
        << VRmixin::theAgent->getOrientation() << std::endl;
          PilotRequest &req = *VRmixin::pilot->curReq;
          req.collisionAction = PilotTypes::collisionIgnore;
          
          Point curPos = VRmixin::theAgent->getCentroid();
          AngTwoPi curHead = VRmixin::theAgent->getOrientation();
          Point vec = endPos - curPos;
          float x = vec.coordX(), y = vec.coordY();
          AngSignPi turnAng = AngSignPi(std::atan2(y, x)) - AngSignPi(curHead);
          float dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
          
          MMAccessor<WaypointWalkMC> wp_acc(getMC());
          wp_acc->getWaypointList().clear();
    #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
          std::cout << "Turn " << float(turnAng)/M_PI*180.f << " degree" << std::endl;
          //std::cout << "Turn " << turnAng << " radians" << std::endl;
          wp_acc->addEgocentricWaypoint(0, 0, turnAng, true, 0.f, pushTurnSpeed);
          std::cout << "Walk forward " << dist << " mm" << std::endl;
          wp_acc->addEgocentricWaypoint(dist, 0, 0, true, pushForwardSpeed, 0.f);
    #else
          std::cout << "Turn " << float(turnAng)/M_PI*180.f << " degree" << std::endl;
          //std::cout << "Turn " << turnAng << " radians" << std::endl;
          wp_acc->addEgocentricWaypoint(0, 0, turnAng, true, 0.f, pushTurnSpeed);
          std::cout << "Walk forward " << dist << " mm" << std::endl;
          wp_acc->addEgocentricWaypoint(dist, 0, 0, true, pushForwardSpeed, 0.f);
    #endif
          motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
      }
    };

    class CheckObjectPos : public StateNode {
     public:
      CheckObjectPos(const std::string &nodename="CheckObjectPos") : StateNode(nodename) {}
      virtual void doStart() {
        Point &endPos = getAncestor<PushObjectMachine>()->endPos;
        bool &isObs = getAncestor<PushObjectMachine>()->isObs;
          
        //std::cout << "--------CheckObjectPos" << std::endl;
          PilotRequest &req = *VRmixin::pilot->curReq;
          if ( req.objectMatcher == NULL ) {
      //std::cout << "No objectMatcher specified" << std::endl;
      VRmixin::pilot->curReq->objectShape->setPosition(endPos);
      postStateSuccess();
          } else {
      ShapeRoot localObj = (*req.objectMatcher)(req.objectShape);
      if ( ! localObj.isValid() ) {
        std::cout << "Can't find the object" << std::endl;
        postStateFailure();
      } else {
        localObj->applyTransform(VRmixin::mapBuilder->localToWorldMatrix, unspecified);
        Point pt = Point(localObj->getCentroid().coordX(), localObj->getCentroid().coordY(), 0, allocentric);
        std::cout << "end position: " << pt << std::endl;
        VRmixin::pilot->curReq->objectShape->setPosition(pt);
        VRmixin::pilot->curReq->objectShape->setObstacle(isObs);
        if (pt.xyDistanceFrom(endPos) < 100) {
          std::cout << "Push finish" << std::endl;
          postStateSuccess();
        }
        else {
          std::cout << "To much error on end position" << std::endl;
          postStateSuccess();
        }
      }
          }
      }
    };

    class MoveObjectPos : public StateNode {
     public:
      MoveObjectPos(const std::string &nodename="MoveObjectPos") : StateNode(nodename) {}
      virtual void doStart() {
        Point &endPos = getAncestor<PushObjectMachine>()->endPos;
    
        //std::cout << "--------MoveObjectPos" << std::endl;
          //Should using vision first
          VRmixin::pilot->curReq->objectShape->setPosition(endPos);
      }
    };

    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);

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

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

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

     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);

     AddBackupPt *plandelivery2 = new AddBackupPt("plandelivery2");
     addNode(plandelivery2);

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

     SetUpObjToDest2 *setupobjtodest21 = new SetUpObjToDest2("setupobjtodest21");
     addNode(setupobjtodest21);

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

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

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

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

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

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

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

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

     DisplayStraightLineInitToObj *displaystraightlineinittoobj1 = new DisplayStraightLineInitToObj("displaystraightlineinittoobj1");
     addNode(displaystraightlineinittoobj1);

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

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

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

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

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

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

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

     DisplayPathInitToObj *displaypathinittoobj1 = new DisplayPathInitToObj("displaypathinittoobj1");
     addNode(displaypathinittoobj1);

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

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

     FetchObj *fetch = new FetchObj("fetch");
     addNode(fetch);

     ChoosePathObjToDest *planpush = new ChoosePathObjToDest("planpush");
     addNode(planpush);

     DisplayStraightLineObjToDest *displaystraightlineobjtodest1 = new DisplayStraightLineObjToDest("displaystraightlineobjtodest1");
     addNode(displaystraightlineobjtodest1);

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

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

     DisplayPathObjToDest *displaypathobjtodest1 = new DisplayPathObjToDest("displaypathobjtodest1");
     addNode(displaypathobjtodest1);

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

     AdjustPush *adjustpush = new AdjustPush("adjustpush");
     addNode(adjustpush);

     WalkBackward *walkbackward1 = new WalkBackward("walkbackward1");
     addNode(walkbackward1);
     walkbackward1->setMC(VRmixin::pilot->waypointwalk_id);

     LookForTarget *lookforobj2 = new LookForTarget("lookforobj2");
     addNode(lookforobj2);

     CheckObjectPos *checkobj = new CheckObjectPos("checkobj");
     addNode(checkobj);

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

     startnode = setmaxturn2;

     setmaxturn2->addTransition(new NullTrans("nulltrans13",checkparams));
     checkparams->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans26",localize,StateNode::successSignal));
     checkparams->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans27",reportcompletion9,StateNode::failureSignal));
     localize->addTransition(new NullTrans("nulltrans14",locutil));
     locutil->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans28",plandelivery,StateNode::successSignal));
     locutil->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans29",plandelivery,StateNode::failureSignal));
     plandelivery->addTransition(new NullTrans("nulltrans15",setupobjtodest1));
     setupobjtodest1->addTransition(new NullTrans("nulltrans16",planpath3));
     planpath3->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans30",reportcompletion10,GenericRRTBase::START_COLLIDES));
     planpath3->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans31",reportcompletion11,GenericRRTBase::END_COLLIDES));
     planpath3->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans32",reportcompletion12,GenericRRTBase::MAX_ITER));
     planpath3->addTransition(new CompletionTrans("completiontrans17",plandelivery2));
     planpath3->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans33",reportcompletion13,StateNode::failureSignal));
     plandelivery2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans34",addacquire,StateNode::successSignal));
     addacquire->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans35",setupobjtodest21,StateNode::successSignal));
     setupobjtodest21->addTransition(new NullTrans("nulltrans17",planpath2));
     plandelivery2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans36",reportcompletion14,StateNode::failureSignal));
     addacquire->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans37",reportcompletion15,StateNode::failureSignal));
     planpath2->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans38",reportcompletion16,GenericRRTBase::START_COLLIDES));
     planpath2->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans39",reportcompletion17,GenericRRTBase::END_COLLIDES));
     planpath2->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans40",reportcompletion18,GenericRRTBase::MAX_ITER));
     planpath2->addTransition(new CompletionTrans("completiontrans18",planacquire));
     planpath2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans41",reportcompletion19,StateNode::failureSignal));
     planacquire->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans42",displaystraightlineinittoobj1,StateNode::successSignal));
     displaystraightlineinittoobj1->addTransition(new NullTrans("nulltrans18",lookforobj));
     planacquire->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans43",setupinittoobj1,StateNode::failureSignal));
     setupinittoobj1->addTransition(new NullTrans("nulltrans19",planpath1));
     planpath1->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans44",reportcompletion20,GenericRRTBase::START_COLLIDES));
     planpath1->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans45",reportcompletion21,GenericRRTBase::END_COLLIDES));
     planpath1->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans46",reportcompletion22,GenericRRTBase::MAX_ITER));
     planpath1->addTransition(new CompletionTrans("completiontrans19",execute));
     planpath1->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans47",reportcompletion23,StateNode::failureSignal));
     execute->addTransition(new CompletionTrans("completiontrans20",displaypathinittoobj1));
     displaypathinittoobj1->addTransition(new NullTrans("nulltrans20",executeplan1));
     executeplan1->addTransition(new CompletionTrans("completiontrans21",lookforobj));
     lookforobj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans48",fetch,StateNode::failureSignal));
     lookforobj->addTransition(new CompletionTrans("completiontrans22",fetch));
     fetch->addTransition(new CompletionTrans("completiontrans23",planpush));
     planpush->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans49",displaystraightlineobjtodest1,StateNode::successSignal));
     displaystraightlineobjtodest1->addTransition(new NullTrans("nulltrans21",adjustpush));
     planpush->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans50",setupexecute21,StateNode::failureSignal));
     setupexecute21->addTransition(new NullTrans("nulltrans22",execute2));
     execute2->addTransition(new CompletionTrans("completiontrans24",displaypathobjtodest1));
     displaypathobjtodest1->addTransition(new NullTrans("nulltrans23",executeplan2));
     executeplan2->addTransition(new CompletionTrans("completiontrans25",adjustpush));
     adjustpush->addTransition(new CompletionTrans("completiontrans26",walkbackward1));
     walkbackward1->addTransition(new CompletionTrans("completiontrans27",lookforobj2));
     lookforobj2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans51",checkobj,StateNode::failureSignal));
     lookforobj2->addTransition(new CompletionTrans("completiontrans28",checkobj));
     checkobj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans52",complete,StateNode::successSignal));
     checkobj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans53",complete,StateNode::failureSignal));
    }

    NavigationPlan initToObjPlan;   // from $provide
    NavigationPlan objToDestPlan;   // from $provide
    Point startPos;   // from $provide
    Point objPos;   // from $provide
    Point endPos;   // from $provide
    AngTwoPi startOri;   // from $provide
    AngTwoPi acquireOri;   // from $provide
    AngTwoPi backupOri;   // from $provide
    Point backupPt;   // from $provide
    Point acquirePt;   // from $provide
    bool useSimpleWayObjToDest;   // from $provide
    bool useSimpleWayInitToObj;   // from $provide
    bool isObs;   // 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 WalkNode {
     public:
      Rotate(const std::string &nodename="Rotate") : WalkNode(nodename) {}
      virtual void doStart() {
          getMC()->setTargetDisplacement(0,0,VRmixin::pilot->curReq->searchRotationAngle,0);
      }
    };

    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->walk_id);

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


  class SetOdometryMachine : public StateNode {
   public:
    SetOdometryMachine(const std::string &nodename="SetOdometryMachine") : StateNode(nodename), turn1_(NULL), turn2_(NULL) {}

    class TurnHead : public HeadPointerNode, public CurrVisualOdometry {
     public:
      TurnHead(const std::string &nodename, float _angle) : HeadPointerNode(nodename), CurrVisualOdometry(nodename), angle(_angle) {}
      float angle;  // cache the constructor's parameter
      
        virtual void preStart() {
    HeadPointerNode::preStart();
   #ifdef TGT_HAS_HEAD
    getMC()->setMaxSpeed(RobotInfo::PanOffset, 1.5f);
   #endif
        }
  
        virtual void doStart() {
    HeadPointerNode::doStart();
    update(true);
   #ifdef TGT_HAS_HEAD
    getMC()->setJointValue(RobotInfo::PanOffset, mathutils::deg2rad(angle));
   #endif
        }
  
        virtual void doEvent() {
    HeadPointerNode::doEvent();
    postStateCompletion();
        }
  
        virtual void doStop() {
    update(true);
    std::cout << "Translation: " << getTranslation() << std::endl;
        }
    };

    TurnHead *turn1_;
    TurnHead *turn2_;

    virtual void doStop() {
      // CreateMotionModel<LocalizationParticle> *model = 
      //   (CreateMotionModel<LocalizationParticle>*)VRmixin::particleFilter->getMotionModel();
      // model->getOdo().setConversionParameters(turn1_->getTranslation()/15.0f, 0.0f);
      if ( VRmixin::imageOdometry != NULL ) {
  std::cout << "Setting visual odometry parameters" << std::endl;
  VRmixin::imageOdometry->setConversionParameters(turn1_->getTranslation()/5.0f, 0.0f);
      }
    }

    virtual void setup() {
     TurnHead *turn1 = new TurnHead("turn1",5.0f);
     addNode(turn1);

     TurnHead *turn2 = new TurnHead("turn2",0.0f);
     addNode(turn2);

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

     startnode = turn1;

     turn1->addTransition(new TimeOutTrans("timeouttrans3",turn2,2000));
     turn2->addTransition(new TimeOutTrans("timeouttrans4",reportcompletion24,2000));

      turn1_ = turn1;
      turn2_ = turn2;
    }
   private:  // dummy copy and assignment to suppress compiler warnings
    SetOdometryMachine(const SetOdometryMachine&);
    SetOdometryMachine& operator=(const SetOdometryMachine&);
  };


  //================ 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), setOdometryMachine_(NULL), reportSuccess_(NULL) {}

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

    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;

      case setOdometry:
  setOdometryMachine_->start();
  break;

      case noRequest:
        reportSuccess_->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 *reportcompletion25 = new ReportCompletion("reportcompletion25",cantLocalize);
     addNode(reportcompletion25);

     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);

     SetOdometryMachine *setOdometryMachine = new SetOdometryMachine("setOdometryMachine");
     addNode(setOdometryMachine);

     startnode = dispatchDummyStart;

     localizemachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans54",reportSuccess,StateNode::successSignal));
     localizemachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans55",reportcompletion25,StateNode::failureSignal));
     waypointwalkmachine->addTransition(new CompletionTrans("completiontrans31",reportSuccess));
     waypointwalkmachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans56",reportFailure,StateNode::failureSignal));
     gotoshapemachine->addTransition(new CompletionTrans("completiontrans32",reportSuccess));
     gotoshapemachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans57",reportFailure,StateNode::failureSignal));
     pushobjectmachine->addTransition(new CompletionTrans("completiontrans33",reportSuccess));
     pushobjectmachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans58",reportFailure,StateNode::failureSignal));
     visualsearchmachine->addTransition(new CompletionTrans("completiontrans34",reportSuccess));
     visualsearchmachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans59",reportFailure,StateNode::failureSignal));
     setOdometryMachine->addTransition(new CompletionTrans("completiontrans35",reportSuccess));

      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;
      setOdometryMachine_ = setOdometryMachine;
      reportSuccess_ = reportSuccess;
    }
   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 NavigationStep &step);
std::ostream& operator<<(std::ostream& os, const NavigationPlan &plan);

} // namespace

#endif