Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Pilot.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_Pilot_h_
00003 #define INCLUDED_Pilot_h_
00004 
00005 // Using a single WaypointWalkMC causes problems if we chain a couple
00006 // of WaypointWalkNodes together because some of the LocomotionEvents
00007 // indicating termination of the first one may not arrive until after
00008 // the second node has been activated, causing the second node to
00009 // terminate prematurely.  The only reason to use shared MCs is for
00010 // the Chiara's XWalk, which runs through an elaborate (slow) leg
00011 // initialization sequence on every start().  No other robots have
00012 // this problem, so the Pilot will only use shared motion commands for
00013 // the Chiara.  For everyone else, waypointwalk_id will be invalid_MC_ID.
00014 #ifdef TGT_IS_CHIARA
00015 #  define PILOT_USES_SHARED_MOTION_COMMANDS
00016 #endif
00017 
00018 // Try using shared motion commands with Calliope
00019 #  define PILOT_USES_SHARED_MOTION_COMMANDS
00020 
00021 
00022 #include <queue>
00023 #include <deque>
00024 
00025 // Have to do all our #includes explicitly; can't rely on
00026 // StateMachine.h because of circular dependencies with the Pilot.
00027 
00028 #include "Behaviors/Nodes/HeadPointerNode.h"
00029 #include "Behaviors/Nodes/PostMachine.h"
00030 #include "Behaviors/Nodes/PostureNode.h"
00031 #include "Behaviors/Nodes/SpeechNode.h"
00032 #include "Behaviors/Nodes/WaypointWalkNode.h"
00033 #include "Behaviors/Nodes/WalkNode.h"
00034 
00035 #include "Behaviors/Transitions/CompletionTrans.h"
00036 #include "Behaviors/Transitions/EventTrans.h"
00037 #include "Behaviors/Transitions/NullTrans.h"
00038 #include "Behaviors/Transitions/PilotTrans.h"
00039 #include "Behaviors/Transitions/SignalTrans.h"
00040 #include "Behaviors/Transitions/TextMsgTrans.h"
00041 #include "Behaviors/Transitions/TimeOutTrans.h"
00042 
00043 #include "Crew/MapBuilderNode.h"
00044 #include "Crew/PilotRequest.h"
00045 
00046 #include "Motion/MMAccessor.h"
00047 #include "Motion/WaypointWalkMC.h"
00048 #include "Motion/WaypointEngine.h"
00049 #include "Motion/WaypointList.h"
00050 
00051 #include "Planners/RRT/GenericRRT.h"
00052 
00053 #include "Vision/VisualOdometry/VisualOdometry.h"
00054 
00055 #include "DualCoding/DualCoding.h"
00056 
00057 using namespace std;
00058 
00059 namespace DualCoding {
00060 
00061 using namespace PilotTypes;
00062 
00063 //! The Pilot is the top-level navigation engine for Tekkotsu.
00064 /*! The Pilot accepts and executes PilotRequest instances.
00065 
00066   Navigation involves a combination of path planning, odometry, and
00067   landmark-based localization.  The Pilot uses the
00068   ShapeSpacePlannerXYTheta planner to find a path from the robot's
00069   current position to a target location that avoids collisions with
00070   worldShS shapes marked as obstacles.
00071 
00072   Once a path is found, the Pilot formulates a <em>navigation
00073   plan</em> consisting of a series of travel steps, turn steps, and
00074   localization steps.  The navigation planner is somewhat clever about
00075   where it places localization steps in order to avoid slowing down
00076   the robot's progress too much, but additional optimizations are
00077   possible.
00078 
00079   Finally, the Pilot executes the navigation plan.  Because both
00080   motion and odometry are subject to error, the robot can drift off
00081   course as it travels.  When this is detected via a localization
00082   step, the execution engine attempts a correction by inserting a new
00083   turn step in the navigation plan and altering the travel step that
00084   follows.
00085   
00086  */
00087 
00088 class Pilot : public StateNode {
00089  public:
00090   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() {}
00091 
00092   typedef unsigned int PilotVerbosity_t;
00093   static const PilotVerbosity_t PVstart =     1<<0;
00094   static const PilotVerbosity_t PVevents =    1<<1;
00095   static const PilotVerbosity_t PVexecute =   1<<2;
00096   static const PilotVerbosity_t PVsuccess =   1<<3;
00097   static const PilotVerbosity_t PVfailure =   1<<4;
00098   static const PilotVerbosity_t PVcomplete =  1<<5;
00099   static const PilotVerbosity_t PVabort =     1<<6;
00100   static const PilotVerbosity_t PVpop =       1<<7;
00101   static const PilotVerbosity_t PVqueued =    1<<8;
00102   static const PilotVerbosity_t PVcollision = 1<<9;
00103   static const PilotVerbosity_t PVnavStep =   1<<10;
00104   static const PilotVerbosity_t PVshowPath =  1<<11;
00105 
00106   static PilotVerbosity_t getVerbosity() { return verbosity; }
00107   static void setVerbosity(PilotVerbosity_t v) { verbosity = v; }
00108 
00109   //! Functor used to sort gaze points by their relative bearing from the robot
00110   struct BearingLessThan {
00111     AngTwoPi hdg;
00112     BearingLessThan(const AngTwoPi heading) : hdg(heading) {}
00113 
00114     bool operator()(AngTwoPi a, AngTwoPi b) {
00115       AngTwoPi aBearing = a - hdg;
00116       AngTwoPi bBearing = b - hdg;
00117       return aBearing < bBearing;
00118     }
00119   };
00120 
00121 
00122   virtual void doStart();
00123   virtual void doStop();
00124   virtual void doEvent();
00125   void unwindForNextRequest();
00126   unsigned int executeRequest(BehaviorBase* requestingBehavior, const PilotRequest& req);
00127   void executeRequest();
00128   void requestComplete(PilotTypes::ErrorType_t errorType);
00129   void requestComplete(const PilotEvent &e);
00130   void pilotPop();
00131   void pilotAbort();
00132   void changeVelocity(float vx, float vy=0, float va=0);  //!< Change velocity for walk or setVelocity requests
00133   void cancelVelocity(); //!< forces completion of a setVelocity operation
00134   void setWorldBounds(float minX, float width, float minY, float height);
00135   void setWorldBounds(const Shape<PolygonData> &bounds); //!< Set world bounds to constrain the path planner
00136   void randomizeParticles(float widthIncr=1000, float heightIncr=1000); //!< Randomize the particles before localization
00137   void computeParticleBounds(float widthIncr=0, float heightIncr=0); //!< Compute bounds for random particle distribution based on world shapes
00138 
00139   void setDefaultLandmarks(const std::vector<ShapeRoot> &landmarks);
00140   const std::vector<ShapeRoot>& getDefaultLandmarks() { return defaultLandmarks; }
00141 
00142   void setDefaultLandmarkExtractor(const MapBuilderRequest &mapreq);
00143   MapBuilderRequest* getDefaultLandmarkExtractor() const { return defaultLandmarkExtractor; }
00144 
00145   MotionManager::MC_ID getWalk_id() const { return walk_id; }
00146   MotionManager::MC_ID getWaypointwalk_id() const { return waypointwalk_id; }
00147 
00148   void setupLandmarkExtractor();
00149   //! Find the landmarks that should be visible from current location
00150   static std::vector<ShapeRoot> calculateVisibleLandmarks(const DualCoding::Point &currentLocation, 
00151                 AngTwoPi currentOrientation, AngTwoPi maxTurn,
00152                 const std::vector<DualCoding::ShapeRoot> &possibleLandmarks);
00153   static bool isLandmarkViewable(const DualCoding::ShapeRoot &selectedLandmark,
00154          const DualCoding::Point &currentLocation, AngTwoPi currentOrientation, AngTwoPi maxTurn);
00155 
00156   //! Returns true if we have enough landmarks to localize.
00157   /*! This could be much more sophisticated: taking landmark geometry
00158    into account so we don't settle for two adjacent landmarks if a
00159    more widely dispersed set is available.
00160   */
00161   static bool defaultLandmarkExitTest();
00162 
00163   //! Pilot's setAgent is called by its RunMotionModel behavior.  Or user can call directly.
00164   /* If @a quiet is false, the new agent position will be announced on
00165      cout. If @a updateWayPoint is true, the Pilot's WaypointWalk
00166      engine's position will also be updated.  This doesn't seem to be a good idea,
00167      as it leads to fights due to the particle filter preventing the Waypoint Walk
00168      engine from reaching its destination.  Needs further study.
00169    */
00170   void setAgent(const Point &loc, AngTwoPi heading, bool quiet=false, bool updateWaypoint=true);
00171 
00172   //================ Background processes ================
00173 
00174   //! Run the particle filter's motion model every few hundred milliseconds to provide odometry
00175   class RunMotionModel : public StateNode {
00176    public:
00177     RunMotionModel(const std::string &nodename="RunMotionModel") : StateNode(nodename) {}
00178     virtual void doStart();
00179     virtual void doEvent();
00180   };
00181 
00182   //! Compute optical flow for visual odometry.  Currently disabled.
00183   class RunOpticalFlow : public StateNode {
00184    public:
00185     RunOpticalFlow(const std::string &nodename="RunOpticalFlow") : StateNode(nodename) {}
00186     virtual void doStart();
00187     virtual void doEvent();
00188   };
00189 
00190   //! Collision checker service; its behavior is model-dependent.
00191   /*! On the Create and Calliope it uses the bump switches and motor torques
00192    */
00193   class CollisionChecker : public StateNode {
00194    public:
00195     CollisionChecker(const std::string &nodename="CollisionChecker") : StateNode(nodename), overcurrentCounter(0) {}
00196     virtual void doStart();
00197     virtual void doEvent();
00198     virtual void enableDetection();
00199     virtual void disableDetection(unsigned int howlong);
00200     virtual void reportCollision();
00201     int overcurrentCounter; //!< count Create overcurrent button events to eliminate transients
00202     static unsigned int const collisionEnableTimer = 1;
00203     static unsigned int const overcurrentResetTimer = 2;
00204   };
00205 
00206   //================ Utility Classes Used By Pilot State Machines ================
00207 
00208   class ReportCompletion : public StateNode {
00209    public:
00210     ReportCompletion(const std::string &nodename, PilotTypes::ErrorType_t _errorType=noError) : StateNode(nodename), errorType(_errorType) {}
00211     PilotTypes::ErrorType_t errorType;  // cache the constructor's parameter
00212     virtual void doStart() {
00213       VRmixin::isWalkingFlag = false;
00214       VRmixin::pilot->requestComplete(errorType);
00215     }
00216   };
00217 
00218   //! Used by Pilot walk machines to decide how to respond to a collision
00219   class CollisionDispatch : public StateNode {
00220    public:
00221     CollisionDispatch(const std::string &nodename="CollisionDispatch") : StateNode(nodename) {}
00222     virtual void doStart();
00223     virtual void doEvent();
00224   };
00225 
00226   //! Terminate a walk operation and indicate that a collision was responsible
00227   class TerminateDueToCollision : public StateNode {
00228    public:
00229     TerminateDueToCollision(const std::string &nodename="TerminateDueToCollision") : StateNode(nodename) {}
00230     virtual void doStart();
00231   };
00232 
00233   class SetupLandmarkExtractor : public StateNode {
00234    public:
00235     SetupLandmarkExtractor(const std::string &nodename="SetupLandmarkExtractor") : StateNode(nodename) {}
00236     virtual void doStart() {
00237       VRmixin::pilot->setupLandmarkExtractor();
00238     }
00239   };
00240 
00241   class PlanPath : public StateNode {
00242    public:
00243     PlanPath(const std::string &nodename="PlanPath") : StateNode(nodename) {}
00244     virtual void doStart() {
00245     NavigationPlan &plan = getAncestor<Pilot>()->plan;
00246     Point &initPos = getAncestor<Pilot>()->initPos;
00247     AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
00248     PilotRequest &planReq = getAncestor<Pilot>()->planReq;
00249       doPlan(plan, initPos, initHead, planReq);
00250     }
00251     void clearGraphics();
00252     void doPlan(NavigationPlan &plan, Point initPos, AngTwoPi initHead, PilotRequest &req);
00253     void showPath(const vector<RRTNodeXYTheta::NodeValue_t> &path);
00254   };
00255 
00256   //! Construct a navigation plan: mixture of traveling, turning, and localizing steps
00257   class ConstructNavPlan : public StateNode {
00258    public:
00259     ConstructNavPlan(const std::string &nodename="ConstructNavPlan") : StateNode(nodename) {}
00260     virtual void doStart() {
00261     NavigationPlan &plan = getAncestor<Pilot>()->plan;
00262     Point &initPos = getAncestor<Pilot>()->initPos;
00263     AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
00264     PilotRequest &planReq = getAncestor<Pilot>()->planReq;
00265       doAnalysis(plan, initPos, initHead, planReq);
00266     }
00267     void doAnalysis(NavigationPlan &plan, Point initPos, AngTwoPi initHead, PilotRequest &req);
00268   };
00269 
00270   //! Execute a navigation plan; used for both goToShape and pushObject
00271   class ExecutePlan : public StateNode {
00272    public:
00273     ExecutePlan(const std::string &nodename="ExecutePlan") : StateNode(nodename), nextDisplacementInstruction() {}
00274     
00275     //! distance in mm we can be from a destination point and be "there"
00276     static const float allowableDistanceError;
00277     static const float allowableAngularError;
00278 
00279     //! Each turn or travel step in a navigation plan is a DisplacementInstruction
00280     struct DisplacementInstruction {
00281       DisplacementInstruction() : nextPoint(), angleToTurn(0), walkBackward(false) {};
00282       fmat::Column<2> nextPoint;
00283       AngSignTwoPi angleToTurn;
00284       bool walkBackward;
00285     };
00286 
00287     enum TurnAdjustType_t {
00288       localizationAdjust,
00289       collisionAdjust
00290     };
00291 
00292     class NextTask : public StateNode {
00293      public:
00294       NextTask(const std::string &nodename="NextTask") : StateNode(nodename) {}
00295       virtual void doStart() {
00296         NavigationPlan &plan = getAncestor<Pilot>()->plan;
00297           if ( ++plan.currentStep == plan.steps.end() )
00298       postStateCompletion();  // indicate that we're done
00299           else
00300       postStateSuccess();     // indicate that we have a task to perform
00301       }
00302     };
00303 
00304     //! Perform a travel step by walking forward or backward
00305     class Walk : public WalkNode {
00306      public:
00307       Walk(const std::string &nodename="Walk") : WalkNode(nodename) {}
00308       virtual void doStart() {
00309           PilotRequest &curReq = *VRmixin::pilot->curReq;
00310         DisplacementInstruction &nextDisplacementInstruction = getAncestor<ExecutePlan>()->nextDisplacementInstruction;
00311     
00312     #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00313           float vx = (curReq.forwardSpeed > 0) ? curReq.forwardSpeed : 50.f;
00314     #else
00315           // these values are for Chiara
00316           float vx = (curReq.forwardSpeed > 0) ? curReq.forwardSpeed : 15.f;
00317     #endif
00318           float dist = nextDisplacementInstruction.nextPoint.norm();
00319           if ( nextDisplacementInstruction.walkBackward )
00320             dist *= -1;
00321           getMC()->setTargetDisplacement(dist,0,0,vx,0,0);
00322           motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
00323       }
00324     };
00325 
00326     //! Perform a turn step
00327     class Turn : public WalkNode {
00328      public:
00329       Turn(const std::string &nodename="Turn") : WalkNode(nodename) {}
00330       virtual void doStart() {
00331           PilotRequest &curReq = *VRmixin::pilot->curReq;
00332         DisplacementInstruction &nextDisplacementInstruction = getAncestor<ExecutePlan>()->nextDisplacementInstruction;
00333     
00334           // std::cout<<"turning \n";
00335     
00336     #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00337           float va = (curReq.turnSpeed > 0) ? curReq.turnSpeed : 0.25f;
00338     #else
00339           // these values are for Chiara
00340           float va = (curReq.turnSpeed > 0) ? curReq.turnSpeed : 0.1f;
00341     #endif
00342           float turnAngle = nextDisplacementInstruction.angleToTurn;
00343           getMC()->setTargetDisplacement(0,0,turnAngle,0,0,va);
00344           motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
00345       }
00346     };
00347 
00348     //! Perform an adjusting turn if we localized successfully, or a collision occurred
00349     class AdjustHeading : public WalkNode {
00350      public:
00351       AdjustHeading(const std::string &nodename, const ExecutePlan::TurnAdjustType_t _adjustmentType) : WalkNode(nodename), adjustmentType(_adjustmentType) {}
00352       const ExecutePlan::TurnAdjustType_t adjustmentType;  // cache the constructor's parameter
00353         virtual void doStart() {
00354       NavigationPlan &plan = getAncestor<Pilot>()->plan;
00355           doAdjust(plan);
00356         }
00357         virtual void doAdjust(NavigationPlan &plan);
00358     };
00359 
00360     class ExecuteStep : public StateNode {
00361      public:
00362       ExecuteStep(const std::string &nodename="ExecuteStep") : StateNode(nodename) {}
00363         virtual void doStart() {
00364       DisplacementInstruction &nextDisplacementInstruction = getAncestor<ExecutePlan>()->nextDisplacementInstruction;
00365       NavigationPlan &plan = getAncestor<Pilot>()->plan;
00366       bool &pushingObj = getAncestor<Pilot>()->pushingObj;
00367   
00368           doExecute(plan, nextDisplacementInstruction, pushingObj);
00369         }
00370   
00371         virtual void doExecute(NavigationPlan &plan, 
00372                                DisplacementInstruction &nextDisplacementInstruction, 
00373                                const bool pushType);
00374     };
00375 
00376     virtual void setup() {
00377      ExecuteStep *execute = new ExecuteStep("execute");
00378      addNode(execute);
00379 
00380      PostMachineCompletion *postmachinecompletion1 = new PostMachineCompletion("postmachinecompletion1");
00381      addNode(postmachinecompletion1);
00382 
00383      LocalizationUtility *localizer = new LocalizationUtility("localizer");
00384      addNode(localizer);
00385 
00386      AdjustHeading *adjust = new AdjustHeading("adjust",localizationAdjust);
00387      addNode(adjust);
00388      adjust->setMC(VRmixin::pilot->walk_id);
00389 
00390      Walk *walker = new Walk("walker");
00391      addNode(walker);
00392      walker->setMC(VRmixin::pilot->walk_id);
00393 
00394      Turn *turner = new Turn("turner");
00395      addNode(turner);
00396      turner->setMC(VRmixin::pilot->walk_id);
00397 
00398      Turn *pretravel = new Turn("pretravel");
00399      addNode(pretravel);
00400      pretravel->setMC(VRmixin::pilot->walk_id);
00401 
00402      NextTask *next = new NextTask("next");
00403      addNode(next);
00404 
00405      PostMachineCompletion *postmachinecompletion2 = new PostMachineCompletion("postmachinecompletion2");
00406      addNode(postmachinecompletion2);
00407 
00408      CollisionDispatch *collisionDispatch = new CollisionDispatch("collisionDispatch");
00409      addNode(collisionDispatch);
00410 
00411      TerminateDueToCollision *term = new TerminateDueToCollision("term");
00412      addNode(term);
00413 
00414      startnode = execute;
00415 
00416      execute->addTransition(new SignalTrans<NavigationStepType_t>("signaltrans1",localizer,localizeStep));
00417      execute->addTransition(new SignalTrans<NavigationStepType_t>("signaltrans2",walker,travelStep));
00418      execute->addTransition(new SignalTrans<NavigationStepType_t>("signaltrans3",turner,turnStep));
00419      execute->addTransition(new SignalTrans<NavigationStepType_t>("signaltrans4",turner,headingStep));
00420      execute->addTransition(new SignalTrans<NavigationStepType_t>("signaltrans5",pretravel,preTravelStep));
00421      execute->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans6",next,StateNode::successSignal));
00422      execute->addTransition(new CompletionTrans("completiontrans1",postmachinecompletion1));
00423      localizer->addTransition(new CompletionTrans("completiontrans2",adjust));
00424      localizer->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans7",next,StateNode::failureSignal));
00425      adjust->addTransition(new CompletionTrans("completiontrans3",next));
00426      walker->addTransition(new CompletionTrans("completiontrans4",next));
00427      turner->addTransition(new CompletionTrans("completiontrans5",next));
00428      pretravel->addTransition(new CompletionTrans("completiontrans6",execute));
00429      next->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans8",execute,StateNode::successSignal));
00430      next->addTransition(new CompletionTrans("completiontrans7",postmachinecompletion2));
00431 
00432      PilotTrans *pilottrans1 = new PilotTrans("pilottrans1",term,PilotTypes::collisionDetected);
00433      collisionDispatch->addTransition(pilottrans1);
00434      localizer->addTransition(pilottrans1);
00435      walker->addTransition(pilottrans1);
00436      turner->addTransition(pilottrans1);
00437 
00438     }
00439     DisplacementInstruction nextDisplacementInstruction;   // from $provide
00440   };
00441   
00442   //================ LocalizationUtility ================
00443 
00444   //! State machine called by various Pilot functions to perform a landmark search and localization.
00445   class LocalizationUtility : public StateNode {
00446    public:
00447     LocalizationUtility(const std::string &nodename="LocalizationUtility") : StateNode(nodename), initialHeading(), gazePoints(), gazeHeadings() {}
00448 
00449 
00450     class TakeInitialPicture : public MapBuilderNode {
00451      public:
00452       TakeInitialPicture(const std::string &nodename="TakeInitialPicture") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
00453       virtual void doStart() {
00454         AngTwoPi &initialHeading = getAncestor<LocalizationUtility>()->initialHeading;
00455           initialHeading = VRmixin::theAgent->getOrientation();
00456         AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
00457           maxTurn = M_PI;
00458           if ( VRmixin::pilot->curReq == NULL // needed to prevent crash if behavior is shut down in the ControllerGUI
00459                || VRmixin::pilot->curReq->landmarkExtractor == NULL ) {
00460             cancelThisRequest();
00461             return;
00462           }
00463           mapreq = *VRmixin::pilot->curReq->landmarkExtractor;
00464           mapreq.clearLocal = true;
00465       }
00466     };
00467 
00468     class CheckEnoughLandmarks : public StateNode {
00469      public:
00470       CheckEnoughLandmarks(const std::string &nodename="CheckEnoughLandmarks") : StateNode(nodename) {}
00471       virtual void doStart() {
00472           if ( VRmixin::pilot->curReq != NULL // needed to prevent crash if behavior is shut down in the ControllerGUI
00473                && VRmixin::pilot->curReq->landmarkExitTest() )
00474             postStateSuccess();
00475           else
00476             postStateFailure();
00477       }
00478     };
00479 
00480 #ifdef TGT_HAS_HEAD
00481     // Use gazepoints since we have a pan/tilt available
00482 
00483     class ChooseGazePoints : public StateNode {
00484      public:
00485       ChooseGazePoints(const std::string &nodename="ChooseGazePoints") : StateNode(nodename) {}
00486         virtual void doStart() {
00487       std::deque<Point> &gazePoints = getAncestor<LocalizationUtility>()->gazePoints;
00488           setupGazePoints(gazePoints);
00489         }
00490         void setupGazePoints(std::deque<Point> &gazePoints);
00491     };
00492 
00493     class PrepareForNextGazePoint : public StateNode {
00494      public:
00495       PrepareForNextGazePoint(const std::string &nodename="PrepareForNextGazePoint") : StateNode(nodename) {}
00496         virtual void doStart() {
00497     // This node just does a quick check because we'll be using the
00498     // pan/tilt for the actual work.  The no-head version (below)
00499     // will turn the body.
00500       std::deque<Point> &gazePoints = getAncestor<LocalizationUtility>()->gazePoints;
00501           if ( gazePoints.empty() )
00502             postStateFailure();
00503           else
00504             postStateCompletion();
00505         }
00506         void setMC(const MotionManager::MC_ID) {}  // dummy for compatibility with headless case below
00507     };
00508 
00509     class TakeNextPicture : public MapBuilderNode {
00510      public:
00511       TakeNextPicture(const std::string &nodename="TakeNextPicture") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
00512       virtual void doStart() {
00513           {
00514             GET_SHAPE(gazePoint, PointData, VRmixin::localShS);
00515             if ( gazePoint.isValid() )
00516               VRmixin::localShS.deleteShape(gazePoint);
00517           }
00518           // send the MapBuilder to the next gaze point
00519         std::deque<Point> &gazePoints = getAncestor<LocalizationUtility>()->gazePoints;
00520           NEW_SHAPE(gazePoint, PointData, new PointData(VRmixin::localShS, gazePoints.front()));
00521           //std::cout << "Pilot localizer moving to gazepoint " << gazePoints.front() << std::endl;
00522           gazePoints.pop_front();
00523           mapreq = *VRmixin::pilot->curReq->landmarkExtractor;
00524           mapreq.searchArea = gazePoint;
00525           mapreq.clearLocal = false;
00526       }
00527     };
00528 
00529     class ReturnToInitialHeading : public StateNode {
00530      public:
00531       ReturnToInitialHeading(const std::string &nodename="ReturnToInitialHeading") : StateNode(nodename) {}
00532         virtual void doStart() {
00533           // dummy node since we never moved the body (because we have a pan/tilt)
00534           postStateCompletion();
00535         }
00536         void setMC(const MotionManager::MC_ID) {}  // dummy for compatibility with headless case below
00537     };
00538 
00539 #else
00540     // No pan/tilt available; must turn the body to acquire more landmarks
00541 
00542     class ChooseGazePoints : public StateNode {
00543      public:
00544       ChooseGazePoints(const std::string &nodename="ChooseGazePoints") : StateNode(nodename) {}
00545         virtual void doStart() {
00546       AngTwoPi &initialHeading = getAncestor<LocalizationUtility>()->initialHeading;
00547       std::deque<AngTwoPi> &gazeHeadings = getAncestor<LocalizationUtility>()->gazeHeadings;
00548       AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
00549   
00550     setupGazeHeadings(initialHeading, gazeHeadings, maxTurn);
00551         }
00552         void setupGazeHeadings(const AngTwoPi initialHeading, std::deque<AngTwoPi> &gazeHeadings, const AngTwoPi maxTurn);
00553     };
00554 
00555     class PrepareForNextGazePoint : public WalkNode {
00556      public:
00557       PrepareForNextGazePoint(const std::string &nodename="PrepareForNextGazePoint") : WalkNode(nodename) {}
00558         virtual void doStart() {
00559       std::deque<AngTwoPi> &gazeHeadings = getAncestor<LocalizationUtility>()->gazeHeadings;
00560       AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
00561     prepareForNext(gazeHeadings, maxTurn);
00562         }
00563         void prepareForNext(std::deque<AngTwoPi> &gazeHeadings, const AngTwoPi maxTurn);
00564     };
00565 
00566     class TakeNextPicture : public MapBuilderNode {
00567      public:
00568       TakeNextPicture(const std::string &nodename="TakeNextPicture") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
00569       virtual void doStart() {
00570           mapreq =  *VRmixin::pilot->curReq->landmarkExtractor;
00571           mapreq.clearLocal = false;
00572         AngTwoPi &initialHeading = getAncestor<LocalizationUtility>()->initialHeading;
00573           const AngTwoPi curHeading = VRmixin::theAgent->getOrientation();
00574           const float headingShift = float(curHeading-initialHeading);
00575           mapreq.baseTransform.rotation() = fmat::rotationZ(headingShift);
00576     
00577       }
00578     };
00579 
00580     class ReturnToInitialHeading : public WalkNode {
00581      public:
00582       ReturnToInitialHeading(const std::string &nodename="ReturnToInitialHeading") : WalkNode(nodename) {
00583           setMC(VRmixin::pilot->getWaypointwalk_id());
00584       }
00585   
00586         virtual void doStart() {
00587     static const float allowableAngularError = 0.10f;
00588       AngTwoPi &initialHeading = getAncestor<LocalizationUtility>()->initialHeading;
00589     const AngTwoPi curHeading = VRmixin::theAgent->getOrientation();
00590     const AngSignPi turnAmount = AngSignPi(initialHeading - curHeading);
00591     if ( fabs(turnAmount) < allowableAngularError ) {
00592       postStateCompletion();
00593       return;
00594     }
00595     getMC()->setTargetDisplacement(0,0,turnAmount,0);
00596     motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
00597         }
00598     };
00599 
00600 #endif // TGT_HAS_HEAD
00601 
00602     class Localize : public StateNode {
00603      public:
00604       Localize(const std::string &nodename="Localize") : StateNode(nodename) {}
00605         static float const minConfidentWeight;
00606         virtual void doStart();
00607     };
00608 
00609     virtual void setup() {
00610      TakeInitialPicture *take = new TakeInitialPicture("take");
00611      addNode(take);
00612 
00613      CheckEnoughLandmarks *checkEnough = new CheckEnoughLandmarks("checkEnough");
00614      addNode(checkEnough);
00615 
00616      ChooseGazePoints *choosegazepoints1 = new ChooseGazePoints("choosegazepoints1");
00617      addNode(choosegazepoints1);
00618 
00619      PrepareForNextGazePoint *loop = new PrepareForNextGazePoint("loop");
00620      addNode(loop);
00621      loop->setMC(VRmixin::pilot->getWaypointwalk_id());
00622 
00623      StateNode *statenode1 = new StateNode("statenode1");
00624      addNode(statenode1);
00625 
00626      TakeNextPicture *takenextpic = new TakeNextPicture("takenextpic");
00627      addNode(takenextpic);
00628 
00629      CheckEnoughLandmarks *checkEnough2 = new CheckEnoughLandmarks("checkEnough2");
00630      addNode(checkEnough2);
00631 
00632      ReturnToInitialHeading *returntoinitialheading1 = new ReturnToInitialHeading("returntoinitialheading1");
00633      addNode(returntoinitialheading1);
00634      returntoinitialheading1->setMC(VRmixin::pilot->getWaypointwalk_id());
00635 
00636      Localize *loc = new Localize("loc");
00637      addNode(loc);
00638 
00639      PostMachineSuccess *postmachinesuccess1 = new PostMachineSuccess("postmachinesuccess1");
00640      addNode(postmachinesuccess1);
00641 
00642      PostMachineCompletion *postmachinecompletion3 = new PostMachineCompletion("postmachinecompletion3");
00643      addNode(postmachinecompletion3);
00644 
00645      ReturnToInitialHeading *outofoptions = new ReturnToInitialHeading("outofoptions");
00646      addNode(outofoptions);
00647 
00648      PostMachineFailure *postmachinefailure1 = new PostMachineFailure("postmachinefailure1");
00649      addNode(postmachinefailure1);
00650 
00651      PostMachineCompletion *postmachinecompletion4 = new PostMachineCompletion("postmachinecompletion4");
00652      addNode(postmachinecompletion4);
00653 
00654      startnode = take;
00655 
00656      take->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans9",outofoptions,StateNode::failureSignal));
00657      take->addTransition(new EventTrans("eventtrans1",checkEnough,EventBase::mapbuilderEGID,(size_t)take,EventBase::statusETID));
00658      checkEnough->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans10",loc,StateNode::successSignal));
00659      checkEnough->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans11",choosegazepoints1,StateNode::failureSignal));
00660      choosegazepoints1->addTransition(new NullTrans("nulltrans1",loop));
00661      loop->addTransition(new CompletionTrans("completiontrans8",statenode1));
00662      statenode1->addTransition(new TimeOutTrans("timeouttrans1",takenextpic,1000));
00663      loop->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans12",loop,StateNode::successSignal));
00664      loop->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans13",outofoptions,StateNode::failureSignal));
00665      takenextpic->addTransition(new EventTrans("eventtrans2",checkEnough2,EventBase::mapbuilderEGID,(size_t)takenextpic,EventBase::statusETID));
00666      checkEnough2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans14",returntoinitialheading1,StateNode::successSignal));
00667      returntoinitialheading1->addTransition(new CompletionTrans("completiontrans9",loc));
00668      checkEnough2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans15",loop,StateNode::failureSignal));
00669      loc->addTransition(new NullTrans("nulltrans2",postmachinesuccess1));
00670      postmachinesuccess1->addTransition(new NullTrans("nulltrans3",postmachinecompletion3));
00671      outofoptions->addTransition(new CompletionTrans("completiontrans10",postmachinefailure1));
00672      postmachinefailure1->addTransition(new NullTrans("nulltrans4",postmachinecompletion4));
00673     }
00674     AngTwoPi initialHeading;   // from $provide
00675     std::deque<Point> gazePoints;   // from $provide
00676     std::deque<AngTwoPi> gazeHeadings;   // from $provide
00677   };
00678 
00679   //================ LocalizationMachine ================
00680 
00681   class LocalizationMachine : public StateNode {
00682    public:
00683     LocalizationMachine(const std::string &nodename="LocalizationMachine") : StateNode(nodename) {}
00684     virtual void setup() {
00685      SetupLandmarkExtractor *setuplandmarkextractor1 = new SetupLandmarkExtractor("setuplandmarkextractor1");
00686      addNode(setuplandmarkextractor1);
00687 
00688      LocalizationUtility *loc = new LocalizationUtility("loc");
00689      addNode(loc);
00690 
00691      PostMachineSuccess *postmachinesuccess2 = new PostMachineSuccess("postmachinesuccess2");
00692      addNode(postmachinesuccess2);
00693 
00694      PostMachineFailure *postmachinefailure2 = new PostMachineFailure("postmachinefailure2");
00695      addNode(postmachinefailure2);
00696 
00697      startnode = setuplandmarkextractor1;
00698 
00699      setuplandmarkextractor1->addTransition(new NullTrans("nulltrans5",loc));
00700      loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans16",postmachinesuccess2,StateNode::successSignal));
00701      loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans17",postmachinefailure2,StateNode::failureSignal));
00702     }
00703   };
00704 
00705   //================ WalkMachine ================
00706 
00707   class WalkMachine : public StateNode {
00708    public:
00709     WalkMachine(const std::string &nodename="WalkMachine") : StateNode(nodename) {}
00710 
00711     class WalkMachine_Walker : public WalkNode {
00712      public:
00713       WalkMachine_Walker(const std::string &nodename="WalkMachine_Walker") : WalkNode(nodename) {}
00714         virtual void doStart();
00715     };
00716 
00717     class SetWalking : public StateNode {
00718      public:
00719       SetWalking(const std::string &nodename="SetWalking") : StateNode(nodename) {}
00720       virtual void doStart() {
00721           VRmixin::isWalkingFlag = true;
00722           // cout << "Turning On Walk" << endl;
00723       }
00724     };
00725 
00726     virtual void setup() {
00727      startnode = new SetWalking("startnode");
00728      addNode(startnode);
00729 
00730      WalkMachine_Walker *walker = new WalkMachine_Walker("walker");
00731      addNode(walker);
00732      walker->setMC(VRmixin::pilot->getWalk_id());
00733 
00734      ReportCompletion *reportcompletion1 = new ReportCompletion("reportcompletion1");
00735      addNode(reportcompletion1);
00736 
00737      CollisionDispatch *collisionDispatch = new CollisionDispatch("collisionDispatch");
00738      addNode(collisionDispatch);
00739 
00740      TerminateDueToCollision *terminateduetocollision1 = new TerminateDueToCollision("terminateduetocollision1");
00741      addNode(terminateduetocollision1);
00742 
00743      NullTrans *nulltrans6 = new NullTrans("nulltrans6",walker);
00744      nulltrans6->addDestination(collisionDispatch);
00745      startnode->addTransition(nulltrans6);
00746 
00747      walker->addTransition(new CompletionTrans("completiontrans11",reportcompletion1));
00748 
00749      PilotTrans *pilottrans2 = new PilotTrans("pilottrans2",terminateduetocollision1,PilotTypes::collisionDetected);
00750      collisionDispatch->addTransition(pilottrans2);
00751      walker->addTransition(pilottrans2);
00752 
00753     }
00754 
00755   };
00756 
00757   //================ WaypointWalkMachine ================
00758 
00759   class WaypointWalkMachine : public StateNode {
00760    public:
00761     WaypointWalkMachine(const std::string &nodename="WaypointWalkMachine") : StateNode(nodename) {}
00762 
00763     class WaypointWalkMachine_WaypointWalker : public WaypointWalkNode {
00764      public:
00765       WaypointWalkMachine_WaypointWalker(const std::string &nodename="WaypointWalkMachine_WaypointWalker") : WaypointWalkNode(nodename) {}
00766         virtual void doStart();
00767     };
00768 
00769     class SetWalking : public StateNode {
00770      public:
00771       SetWalking(const std::string &nodename="SetWalking") : StateNode(nodename) {}
00772       virtual void doStart() {
00773           VRmixin::isWalkingFlag = true;
00774           // cout << "Turning On Walk" << endl;
00775       }
00776     };
00777 
00778     virtual void setup() {
00779      startnode = new StateNode("startnode");
00780      addNode(startnode);
00781 
00782      WaypointWalkMachine_WaypointWalker *walker = new WaypointWalkMachine_WaypointWalker("walker");
00783      addNode(walker);
00784      walker->setMC(VRmixin::pilot->getWaypointwalk_id());
00785 
00786      ReportCompletion *reportcompletion2 = new ReportCompletion("reportcompletion2");
00787      addNode(reportcompletion2);
00788 
00789      CollisionDispatch *collisionDispatch = new CollisionDispatch("collisionDispatch");
00790      addNode(collisionDispatch);
00791 
00792      TerminateDueToCollision *terminateduetocollision2 = new TerminateDueToCollision("terminateduetocollision2");
00793      addNode(terminateduetocollision2);
00794 
00795      NullTrans *nulltrans7 = new NullTrans("nulltrans7",walker);
00796      nulltrans7->addDestination(collisionDispatch);
00797      startnode->addTransition(nulltrans7);
00798 
00799      walker->addTransition(new CompletionTrans("completiontrans12",reportcompletion2));
00800 
00801      PilotTrans *pilottrans3 = new PilotTrans("pilottrans3",terminateduetocollision2,PilotTypes::collisionDetected);
00802      collisionDispatch->addTransition(pilottrans3);
00803      walker->addTransition(pilottrans3);
00804 
00805     }
00806 
00807   };
00808 
00809   //=============== SetVelocityMachine ==================
00810   class SetVelocityMachine : public StateNode {
00811    public:
00812     SetVelocityMachine(const std::string &nodename="SetVelocityMachine") : StateNode(nodename) {}
00813 
00814     class SetVelocityMachine_Walker : public WalkNode {
00815      public:
00816       SetVelocityMachine_Walker(const std::string &nodename="SetVelocityMachine_Walker") : WalkNode(nodename) {}
00817         virtual void doStart();
00818     };
00819 
00820     virtual void setup() {
00821      startnode = new StateNode("startnode");
00822      addNode(startnode);
00823 
00824      SetVelocityMachine_Walker *walker = new SetVelocityMachine_Walker("walker");
00825      addNode(walker);
00826      walker->setMC(VRmixin::pilot->getWalk_id());
00827 
00828      StateNode *statenode2 = new StateNode("statenode2");
00829      addNode(statenode2);
00830 
00831      ReportCompletion *reportcompletion3 = new ReportCompletion("reportcompletion3");
00832      addNode(reportcompletion3);
00833 
00834      CollisionDispatch *collisionDispatch = new CollisionDispatch("collisionDispatch");
00835      addNode(collisionDispatch);
00836 
00837      TerminateDueToCollision *terminateduetocollision3 = new TerminateDueToCollision("terminateduetocollision3");
00838      addNode(terminateduetocollision3);
00839 
00840      NullTrans *nulltrans8 = new NullTrans("nulltrans8",walker);
00841      nulltrans8->addDestination(collisionDispatch);
00842      startnode->addTransition(nulltrans8);
00843 
00844      walker->addTransition(new CompletionTrans("completiontrans13",statenode2));
00845      statenode2->addTransition(new TimeOutTrans("timeouttrans2",reportcompletion3,2000));
00846 
00847      PilotTrans *pilottrans4 = new PilotTrans("pilottrans4",terminateduetocollision3,PilotTypes::collisionDetected);
00848      collisionDispatch->addTransition(pilottrans4);
00849      walker->addTransition(pilottrans4);
00850 
00851     }
00852 
00853   };
00854 
00855   //================ GoToShapeMachine ================
00856 
00857   class GoToShapeMachine : public StateNode {
00858    public:
00859     GoToShapeMachine(const std::string &nodename="GoToShapeMachine") : StateNode(nodename) {}
00860 
00861     //! Check that the request's goToShape parameters are valid
00862     class CheckParameters : public StateNode {
00863      public:
00864       CheckParameters(const std::string &nodename="CheckParameters") : StateNode(nodename) {}
00865         virtual void doStart();
00866     };
00867 
00868     class SetUpPlanNode : public StateNode {
00869      public:
00870       SetUpPlanNode(const std::string &nodename="SetUpPlanNode") : StateNode(nodename) {}
00871       virtual void doStart() {
00872         Point &initPos = getAncestor<Pilot>()->initPos;
00873         AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
00874         PilotRequest &planReq = getAncestor<Pilot>()->planReq;
00875         bool &pushingObj = getAncestor<Pilot>()->pushingObj;
00876     
00877             initPos = VRmixin::theAgent->getCentroid();
00878             initHead = VRmixin::theAgent->getOrientation();
00879             planReq = *VRmixin::pilot->curReq;
00880             pushingObj = false;
00881       }
00882     };
00883 
00884     class SetMaxTurn : public StateNode {
00885      public:
00886       SetMaxTurn(const std::string &nodename="SetMaxTurn") : StateNode(nodename) {}
00887       virtual void doStart() {
00888         AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
00889           maxTurn = M_PI;
00890       }
00891     };
00892 
00893     virtual void  setup() {   // setup GoToShapeMachine
00894 
00895      SetMaxTurn *setmaxturn1 = new SetMaxTurn("setmaxturn1");
00896      addNode(setmaxturn1);
00897 
00898      CheckParameters *check = new CheckParameters("check");
00899      addNode(check);
00900 
00901      ReportCompletion *reportcompletion4 = new ReportCompletion("reportcompletion4",invalidRequest);
00902      addNode(reportcompletion4);
00903 
00904      SetupLandmarkExtractor *setuplandmarkextractor2 = new SetupLandmarkExtractor("setuplandmarkextractor2");
00905      addNode(setuplandmarkextractor2);
00906 
00907      LocalizationUtility *loc = new LocalizationUtility("loc");
00908      addNode(loc);
00909 
00910      ClearOldPath *setupplan = new ClearOldPath("setupplan");
00911      addNode(setupplan);
00912 
00913      SetUpPlanNode *setupplannode1 = new SetUpPlanNode("setupplannode1");
00914      addNode(setupplannode1);
00915 
00916      PlanPath *planpath = new PlanPath("planpath");
00917      addNode(planpath);
00918 
00919      ReportCompletion *reportcompletion5 = new ReportCompletion("reportcompletion5",startCollides);
00920      addNode(reportcompletion5);
00921 
00922      ReportCompletion *reportcompletion6 = new ReportCompletion("reportcompletion6",endCollides);
00923      addNode(reportcompletion6);
00924 
00925      ReportCompletion *reportcompletion7 = new ReportCompletion("reportcompletion7",noPath);
00926      addNode(reportcompletion7);
00927 
00928      ReportCompletion *reportcompletion8 = new ReportCompletion("reportcompletion8");
00929      addNode(reportcompletion8);
00930 
00931      CollisionDispatch *collisionDispatch = new CollisionDispatch("collisionDispatch");
00932      addNode(collisionDispatch);
00933 
00934      ConstructNavPlan *constructnavplan = new ConstructNavPlan("constructnavplan");
00935      addNode(constructnavplan);
00936 
00937      ExecutePlan *execute = new ExecutePlan("execute");
00938      addNode(execute);
00939 
00940      PostMachineCompletion *postmachinecompletion5 = new PostMachineCompletion("postmachinecompletion5");
00941      addNode(postmachinecompletion5);
00942 
00943      TerminateDueToCollision *term = new TerminateDueToCollision("term");
00944      addNode(term);
00945 
00946      startnode = setmaxturn1;
00947 
00948      setmaxturn1->addTransition(new NullTrans("nulltrans9",check));
00949      check->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans18",reportcompletion4,StateNode::failureSignal));
00950      check->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans19",setuplandmarkextractor2,StateNode::successSignal));
00951      setuplandmarkextractor2->addTransition(new NullTrans("nulltrans10",setupplan));
00952      loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans20",setupplan,StateNode::successSignal));
00953      loc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans21",setupplan,StateNode::failureSignal));
00954      setupplan->addTransition(new NullTrans("nulltrans11",setupplannode1));
00955      setupplannode1->addTransition(new NullTrans("nulltrans12",planpath));
00956      planpath->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans22",reportcompletion5,GenericRRTBase::START_COLLIDES));
00957      planpath->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans23",reportcompletion6,GenericRRTBase::END_COLLIDES));
00958      planpath->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans24",reportcompletion7,GenericRRTBase::MAX_ITER));
00959      planpath->addTransition(new CompletionTrans("completiontrans14",constructnavplan));
00960      planpath->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans25",reportcompletion8,StateNode::failureSignal));
00961 
00962      CompletionTrans *completiontrans15 = new CompletionTrans("completiontrans15",execute);
00963      completiontrans15->addDestination(collisionDispatch);
00964      constructnavplan->addTransition(completiontrans15);
00965 
00966      execute->addTransition(new CompletionTrans("completiontrans16",postmachinecompletion5));
00967 
00968      PilotTrans *pilottrans5 = new PilotTrans("pilottrans5",term,PilotTypes::collisionDetected);
00969      collisionDispatch->addTransition(pilottrans5);
00970      execute->addTransition(pilottrans5);
00971 
00972     }
00973   };
00974   
00975   class ClearOldPath : public StateNode {
00976    public:
00977     ClearOldPath(const std::string &nodename="ClearOldPath") : StateNode(nodename) {}
00978     virtual void doStart() {
00979       GET_SHAPE(plannedInitToObjPath, GraphicsData, VRmixin::worldShS);
00980       plannedInitToObjPath.deleteShape();
00981       GET_SHAPE(plannedObjToDestPath, GraphicsData, VRmixin::worldShS);
00982       plannedObjToDestPath.deleteShape();
00983       GET_SHAPE(plannedPath, GraphicsData, VRmixin::worldShS);
00984       plannedPath.deleteShape();
00985     }
00986   };
00987   
00988   class LookForObject : public MapBuilderNode {
00989    public:
00990     LookForObject(const std::string &nodename="LookForObject") : MapBuilderNode(nodename) {}
00991     virtual void doStart() {
00992       //MapBuilderRequest *m = VRmixin::pilot->curReq->objectExtractor;
00993       //if ( m == NULL )
00994       //  cancelThisRequest();
00995       //else
00996       //  mapreq = *m;
00997         
00998       MapBuilderRequest *m = new MapBuilderRequest(MapBuilderRequest::localMap);
00999       std::vector<Point> ptVec;
01000       Point pt1 = Point(100, 100, 0, allocentric);
01001       Point pt2 = Point(100, -100, 0, allocentric);
01002       Point pt3 = Point(-100, -100, 0, allocentric);
01003       Point pt4 = Point(-100, 100, 0, allocentric);
01004       Point objPt = VRmixin::pilot->curReq->objectShape->getCentroid();
01005       ptVec.push_back(objPt);
01006       ptVec.push_back(objPt+pt1);
01007       ptVec.push_back(objPt+pt2);
01008       ptVec.push_back(objPt+pt3);
01009       ptVec.push_back(objPt+pt4);
01010       ptVec.push_back(objPt);
01011       NEW_SHAPE_N(polygon, PolygonData, new PolygonData(VRmixin::worldShS, ptVec, false));
01012       polygon->setObstacle(false);
01013       m->searchArea = polygon;
01014       m->addAttributes(VRmixin::pilot->curReq->objectShape);
01015       mapreq = *m;
01016     }
01017   };
01018   
01019   class LookForTarget : public MapBuilderNode {
01020    public:
01021     LookForTarget(const std::string &nodename="LookForTarget") : MapBuilderNode(nodename) {}
01022     virtual void doStart() {
01023       //MapBuilderRequest *m = VRmixin::pilot->curReq->objectExtractor;
01024       //if ( m == NULL )
01025       //  cancelThisRequest();
01026       //else
01027       //  mapreq = *m;
01028         
01029       MapBuilderRequest *m = new MapBuilderRequest(MapBuilderRequest::localMap);
01030       std::vector<Point> ptVec;
01031       Point pt1 = Point(100, 100, 0, allocentric);
01032       Point pt2 = Point(100, -100, 0, allocentric);
01033       Point pt3 = Point(-100, -100, 0, allocentric);
01034       Point pt4 = Point(-100, 100, 0, allocentric);
01035       Point ctPt = VRmixin::pilot->curReq->targetShape->getCentroid();
01036       ptVec.push_back(ctPt);
01037       ptVec.push_back(ctPt+pt1);
01038       ptVec.push_back(ctPt+pt2);
01039       ptVec.push_back(ctPt+pt3);
01040       ptVec.push_back(ctPt+pt4);
01041       ptVec.push_back(ctPt);
01042       NEW_SHAPE_N(polygon, PolygonData, new PolygonData(VRmixin::worldShS, ptVec, false));
01043       polygon->setObstacle(false);
01044       m->searchArea = polygon;
01045       m->addAttributes(VRmixin::pilot->curReq->objectShape);
01046       mapreq = *m;
01047     }
01048   };
01049 
01050   //================ PushObjectMachine ================
01051 
01052   class PushObjectMachine : public StateNode {
01053    public:
01054     PushObjectMachine(const std::string &nodename="PushObjectMachine") : StateNode(nodename), initToObjPlan(), objToDestPlan(), startPos(), objPos(), endPos(), startOri(), acquireOri(), backupOri(), backupPt(), acquirePt(), useSimpleWayObjToDest(), useSimpleWayInitToObj(), isObs() {}
01055   
01056     //static const int backupDist = 600;
01057     //static const int obstDist = 150;
01058     //static const int robotDiam = 150;
01059     //static const int objSize = 50;
01060     static int backupDist;
01061     static int obstDist;
01062     static int robotDiam;
01063     static int objSize;
01064     
01065     static float prePushTurnSpeed;
01066     static float prePushForwardSpeed;
01067     static float pushTurnSpeed;
01068     static float pushForwardSpeed;
01069 
01070     
01071     
01072     
01073     
01074 
01075 
01076     static Point FindFreeSpace(ShapeRoot &targetShape, int targetDistance, int obstacleDistance, AngTwoPi orientation) {
01077       float diagonalLength = sqrt(VRmixin::worldSkS.getWidth()*VRmixin::worldSkS.getWidth() + 
01078           VRmixin::worldSkS.getHeight()*VRmixin::worldSkS.getHeight());
01079       float scale = diagonalLength / (obstacleDistance * 10);
01080       float setx = (VRmixin::worldSkS.getHeight()/2) - targetShape->getCentroid().coordX() * scale;
01081       float sety = (VRmixin::worldSkS.getWidth()/2) - targetShape->getCentroid().coordY() * scale;
01082       VRmixin::worldSkS.setTmat(scale, setx, sety);
01083       bool isObs = targetShape->isObstacle();
01084       targetShape->setObstacle(false);
01085       
01086       NEW_SKETCH_N(obstacles, bool, visops::zeros(VRmixin::worldSkS));
01087       NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(VRmixin::worldShS));
01088       SHAPEVEC_ITERATE(ellipses, EllipseData, ellipse) {
01089         if (ellipse->isObstacle()) {
01090           NEW_SKETCH_N(s1, bool, ellipse->getRendering());
01091           obstacles |= s1;
01092         }
01093       END_ITERATE }
01094       NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(VRmixin::worldShS));
01095       SHAPEVEC_ITERATE(blobs, BlobData, blob) {
01096         if (blob->isObstacle()) {
01097           NEW_SKETCH_N(s1, bool, blob->getRendering());
01098           obstacles |= s1;
01099         }
01100       END_ITERATE }
01101       NEW_SHAPEVEC(lines, LineData, select_type<LineData>(VRmixin::worldShS));
01102       SHAPEVEC_ITERATE(lines, LineData, line) {
01103         if (line->isObstacle()) {
01104           NEW_SKETCH_N(s1, bool, line->getRendering());
01105           obstacles |= s1;
01106         }
01107       END_ITERATE }
01108       
01109       NEW_SKETCH_N(dist, float, visops::edist(obstacles));
01110       NEW_SKETCH_N(cand, bool, dist > obstacleDistance*scale);
01111       //NEW_SKETCH_N(cand, bool, (dist > distance*scale) & (dist < (distance+10)*scale));
01112       
01113       //NEW_SHAPE(point, PointData, new PointData(VRmixin::worldShS, targetPoint));
01114       //NEW_SKETCH_N(target, bool, point->getRendering());
01115       NEW_SKETCH_N(target, bool, targetShape->getRendering());
01116       NEW_SKETCH_N(tdist, float, visops::ebdist(target, !cand, 2*diagonalLength));
01117       NEW_SKETCH_N(cdist, bool, (tdist >= targetDistance*scale) & (tdist <= targetDistance*scale+1));
01118       //NEW_SKETCH_N(cdist, bool, tdist2 & cand);
01119       
01120       AngTwoPi ori = orientation + AngTwoPi(M_PI);
01121       //float length = sqrt(VRmixin::worldSkS.getWidth()*VRmixin::worldSkS.getWidth() + VRmixin::worldSkS.getHeight()*VRmixin::worldSkS.getHeight()) / VRmixin::worldSkS.getScaleFactor();
01122       float length = diagonalLength / scale;
01123       Point bp(targetShape->getCentroid().coordX()+length*std::cos(ori), targetShape->getCentroid().coordY()+length*std::sin(ori), 0, allocentric);
01124       NEW_SHAPE(line, LineData, new LineData(VRmixin::worldShS, targetShape->getCentroid(), bp));
01125       line->setColor("red");
01126       NEW_SKETCH_N(ls, bool, line->getRendering());
01127       NEW_SKETCH_N(lsdist, float, visops::edist(ls));
01128       NEW_SKETCH_N(ldist, float, visops::edist(ls) * cdist);
01129       line.deleteShape();
01130       
01131       int pos = ldist->findMinPlus();
01132       if (pos==-1) return Point(0,0,0,unspecified);
01133       NEW_SKETCH_N(final, bool, visops::zeros(ldist));
01134       final[pos] = true;
01135       final->setColor("green");
01136       targetShape->setObstacle(isObs);
01137       Point pt = final->indexPoint(pos);
01138       std::cout << "Found free space at point: " << pt << std::endl;
01139       return pt;
01140     }
01141     
01142     static Point FindFreeSpace2(ShapeRoot &targetShape, int targetDistance, int obstacleDistance, AngTwoPi orientation) {
01143       float diagonalLength = sqrt(VRmixin::worldSkS.getWidth()*VRmixin::worldSkS.getWidth() + VRmixin::worldSkS.getHeight()*VRmixin::worldSkS.getHeight());
01144       float scale = diagonalLength / (obstacleDistance * 10);
01145       float setx = (VRmixin::worldSkS.getHeight()/2) - targetShape->getCentroid().coordX() * scale;
01146       float sety = (VRmixin::worldSkS.getWidth()/2) - targetShape->getCentroid().coordY() * scale;
01147       VRmixin::worldSkS.setTmat(scale, setx, sety);
01148       bool isObs = targetShape->isObstacle();
01149       targetShape->setObstacle(false);
01150       
01151       NEW_SKETCH(obstacles, bool, visops::zeros(VRmixin::worldSkS));
01152       NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(VRmixin::worldShS));
01153       SHAPEVEC_ITERATE(ellipses, EllipseData, ellipse) {
01154         if (ellipse->isObstacle()) {
01155           NEW_SKETCH(s1, bool, ellipse->getRendering());
01156           obstacles |= s1;
01157         }
01158       END_ITERATE }
01159       NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(VRmixin::worldShS));
01160       SHAPEVEC_ITERATE(blobs, BlobData, blob) {
01161         if (blob->isObstacle()) {
01162           NEW_SKETCH(s1, bool, blob->getRendering());
01163           obstacles |= s1;
01164         }
01165       END_ITERATE }
01166       NEW_SHAPEVEC(lines, LineData, select_type<LineData>(VRmixin::worldShS));
01167       SHAPEVEC_ITERATE(lines, LineData, line) {
01168         if (line->isObstacle()) {
01169           NEW_SKETCH(s1, bool, line->getRendering());
01170           obstacles |= s1;
01171         }
01172       END_ITERATE }
01173       
01174       NEW_SKETCH(dist, float, visops::edist(obstacles));
01175       NEW_SKETCH(cand, bool, dist > obstacleDistance*scale);
01176       //NEW_SKETCH(cand, bool, (dist > distance*scale) & (dist < (distance+10)*scale));
01177       
01178       //NEW_SHAPE(point, PointData, new PointData(VRmixin::worldShS, targetPoint));
01179       //NEW_SKETCH(target, bool, point->getRendering());
01180       NEW_SKETCH(target, bool, targetShape->getRendering());
01181       NEW_SKETCH(tdist, float, visops::ebdist(target, !cand, 2*diagonalLength));
01182       NEW_SKETCH(cdist, bool, (tdist >= targetDistance*scale) & (tdist <= targetDistance*scale+1));
01183       //NEW_SKETCH(cdist, bool, tdist2 & cand);
01184       
01185       AngTwoPi ori = orientation + AngTwoPi(M_PI);
01186       //float length = sqrt(VRmixin::worldSkS.getWidth()*VRmixin::worldSkS.getWidth() + VRmixin::worldSkS.getHeight()*VRmixin::worldSkS.getHeight()) / VRmixin::worldSkS.getScaleFactor();
01187       float length = diagonalLength / scale;
01188       Point bp(targetShape->getCentroid().coordX()+length*std::cos(ori), targetShape->getCentroid().coordY()+length*std::sin(ori), 0, allocentric);
01189       NEW_SHAPE(line, LineData, new LineData(VRmixin::worldShS, targetShape->getCentroid(), bp));
01190       line->setColor("red");
01191       NEW_SKETCH(ls, bool, line->getRendering());
01192       NEW_SKETCH(lsdist, float, visops::edist(ls));
01193       NEW_SKETCH(ldist, float, visops::edist(ls) * cdist);
01194       line.deleteShape();
01195       
01196       int pos = ldist->findMinPlus();
01197       if (pos==-1) return Point(0,0,0,unspecified);
01198       NEW_SKETCH(final, bool, visops::zeros(ldist));
01199       final[pos] = true;
01200       final->setColor("green");
01201       targetShape->setObstacle(isObs);
01202       Point pt = final->indexPoint(pos);
01203       std::cout << "Found free space at point: " << pt << std::endl;
01204       return pt;
01205     }
01206   
01207     class SetMaxTurn : public StateNode {
01208      public:
01209       SetMaxTurn(const std::string &nodename, float _value=M_PI) : StateNode(nodename), value(_value) {}
01210       float value;  // cache the constructor's parameter
01211       virtual void doStart() {
01212         AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
01213           maxTurn = value;
01214       }
01215     };
01216 
01217     class CheckParameters : public StateNode {
01218      public:
01219       CheckParameters(const std::string &nodename="CheckParameters") : StateNode(nodename) {}
01220         virtual void doStart();
01221     };
01222 
01223     class SetUpObjToDest : public StateNode {
01224      public:
01225       SetUpObjToDest(const std::string &nodename="SetUpObjToDest") : StateNode(nodename) {}
01226       virtual void doStart() {
01227         Point &initPos = getAncestor<Pilot>()->initPos;
01228         PilotRequest &planReq = getAncestor<Pilot>()->planReq;
01229         NavigationPlan &plan = getAncestor<Pilot>()->plan;
01230         bool &pushingObj = getAncestor<Pilot>()->pushingObj;
01231         Point &startPos = getAncestor<PushObjectMachine>()->startPos;
01232         Point &objPos = getAncestor<PushObjectMachine>()->objPos;
01233         Point &endPos = getAncestor<PushObjectMachine>()->endPos;
01234         AngTwoPi &startOri = getAncestor<PushObjectMachine>()->startOri;
01235         bool &isObs = getAncestor<PushObjectMachine>()->isObs;
01236           
01237         //std::cout << "--------SetUpObjToDest" << std::endl;
01238           startPos = VRmixin::theAgent->getCentroid();
01239           objPos = VRmixin::pilot->curReq->objectShape->getCentroid();
01240           endPos = VRmixin::pilot->curReq->targetShape->getCentroid();
01241           startOri = VRmixin::theAgent->getOrientation();
01242           
01243           plan.clear();
01244           initPos = objPos;
01245           planReq = *VRmixin::pilot->curReq;
01246           isObs = planReq.objectShape->isObstacle();
01247           planReq.objectShape->setObstacle(false);
01248           pushingObj = false;
01249           planReq.displayPath = false;
01250       }
01251     };
01252 
01253     class AddBackupPt : public StateNode {
01254      public:
01255       AddBackupPt(const std::string &nodename="AddBackupPt") : StateNode(nodename) {}
01256       virtual void doStart() {
01257           /*  NEEDS TO BE UPDATED
01258         NavigationPlan &plan = getAncestor<Pilot>()->plan;
01259         Point &endPos = getAncestor<PushObjectMachine>()->endPos;
01260         Point &backupPt = getAncestor<PushObjectMachine>()->backupPt;
01261         AngTwoPi &backupOri = getAncestor<PushObjectMachine>()->backupOri;
01262           
01263           //std::cout << "--------AddBackupPt" << std::endl;
01264           size_t s = plan.path.size();
01265           Point final = plan.path[s-1];
01266           Point prev = plan.path[s-2];
01267           Point vec = final-prev;
01268           AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
01269           Point temp = VRmixin::pilot->curReq->objectShape->getCentroid();
01270           VRmixin::pilot->curReq->objectShape->setPosition(final);
01271           //std::cout << "tempBackupOri: " << ori << std::endl;
01272           backupPt = FindFreeSpace(VRmixin::pilot->curReq->objectShape, backupDist, obstDist, ori);
01273           VRmixin::pilot->curReq->objectShape->setPosition(temp);
01274           ReferenceFrameType_t t = backupPt.getRefFrameType();
01275           if (t == unspecified) {
01276             std::cout << "Not enough space at destination." << std::endl;
01277             postStateFailure();
01278             return;
01279           }
01280           Point vec2 = endPos - backupPt;
01281           backupOri = AngTwoPi(std::atan2(vec2.coordY(), vec2.coordX()));
01282           //std::cout << "backupPt: " << backupPt << ", backupOri: " << backupOri << std::endl;
01283           */
01284           postStateSuccess();
01285       }
01286     };
01287 
01288     class AddAcquirePt : public StateNode {
01289      public:
01290       AddAcquirePt(const std::string &nodename="AddAcquirePt") : StateNode(nodename) {}
01291       virtual void doStart() {
01292           /* NEEDS TO BE UPDATED
01293         NavigationPlan &plan = getAncestor<Pilot>()->plan;
01294         Point &objPos = getAncestor<PushObjectMachine>()->objPos;
01295         Point &acquirePt = getAncestor<PushObjectMachine>()->acquirePt;
01296         AngTwoPi &acquireOri = getAncestor<PushObjectMachine>()->acquireOri;
01297           
01298           //std::cout << "--------AddAcquirePt" << std::endl;
01299           Point first = plan.path[0];
01300           Point next = plan.path[1];
01301           Point vec = next-first;
01302           AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
01303           //std::cout << "tempAcquireOri: " << ori << std::endl;
01304           acquirePt = FindFreeSpace(VRmixin::pilot->curReq->objectShape, backupDist, obstDist, ori);
01305           ReferenceFrameType_t t = acquirePt.getRefFrameType();
01306           if (t == unspecified) {
01307             std::cout << "Not enough space for acquiring object." << std::endl;
01308             postStateFailure();
01309             return;
01310           }
01311           Point vec2 = objPos - acquirePt;
01312           acquireOri = AngTwoPi(std::atan2(vec2.coordY(), vec2.coordX()));
01313           //std::cout << "acquirePt: " << acquirePt << ", acquireOri: " << acquireOri << std::endl;
01314           */
01315           postStateSuccess();
01316       }
01317     };
01318     
01319     class SetUpObjToDest2 : public StateNode {
01320      public:
01321       SetUpObjToDest2(const std::string &nodename="SetUpObjToDest2") : StateNode(nodename) {}
01322       virtual void doStart() {
01323         Point &initPos = getAncestor<Pilot>()->initPos;
01324         AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
01325         PilotRequest &planReq = getAncestor<Pilot>()->planReq;
01326         NavigationPlan &plan = getAncestor<Pilot>()->plan;
01327         bool &pushingObj = getAncestor<Pilot>()->pushingObj;
01328         Point &backupPt = getAncestor<PushObjectMachine>()->backupPt;
01329         Point &objPos = getAncestor<PushObjectMachine>()->objPos;
01330         AngTwoPi &acquireOri = getAncestor<PushObjectMachine>()->acquireOri;
01331         AngTwoPi &backupOri = getAncestor<PushObjectMachine>()->backupOri;
01332           
01333         //std::cout << "--------SetUpObjToDest2" << std::endl;
01334           plan.clear();
01335           
01336           initPos = objPos;
01337           initHead = acquireOri;
01338           planReq = *VRmixin::pilot->curReq;
01339           planReq.objectShape->setObstacle(false);
01340           pushingObj = true;
01341           NEW_SHAPE(backup, PointData, new PointData(VRmixin::worldShS, backupPt));
01342           backup->setObstacle(false);
01343           planReq.targetShape = backup;
01344           planReq.targetHeading = backupOri;
01345           planReq.displayPath = false;
01346       }
01347     };
01348     
01349     class SetUpInitToObj : public StateNode {
01350      public:
01351       SetUpInitToObj(const std::string &nodename="SetUpInitToObj") : StateNode(nodename) {}
01352       virtual void doStart() {
01353         Point &initPos = getAncestor<Pilot>()->initPos;
01354         AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
01355         PilotRequest &planReq = getAncestor<Pilot>()->planReq;
01356         NavigationPlan &plan = getAncestor<Pilot>()->plan;
01357         bool &pushingObj = getAncestor<Pilot>()->pushingObj;
01358         Point &startPos = getAncestor<PushObjectMachine>()->startPos;
01359         AngTwoPi &startOri = getAncestor<PushObjectMachine>()->startOri;
01360         Point &acquirePt = getAncestor<PushObjectMachine>()->acquirePt;
01361         AngTwoPi &acquireOri = getAncestor<PushObjectMachine>()->acquireOri;
01362           
01363         //std::cout << "--------SetUpInitToObj" << std::endl;
01364           plan.clear();
01365           
01366           initPos = startPos;
01367           initHead = startOri;
01368           planReq = *VRmixin::pilot->curReq;
01369           planReq.objectShape->setObstacle(true);
01370           pushingObj = false;
01371           NEW_SHAPE(acquire, PointData, new PointData(VRmixin::worldShS, acquirePt));
01372           acquire->setObstacle(false);
01373           planReq.targetShape = acquire;
01374           planReq.targetHeading = acquireOri;
01375           planReq.displayPath = false;
01376           planReq.turnSpeed = prePushTurnSpeed;
01377           planReq.forwardSpeed = prePushForwardSpeed;
01378       }
01379     };
01380     
01381     class ChoosePathInitToObj : public StateNode {
01382      public:
01383       ChoosePathInitToObj(const std::string &nodename="ChoosePathInitToObj") : StateNode(nodename) {}
01384       virtual void doStart() {
01385         NavigationPlan &plan = getAncestor<Pilot>()->plan;
01386         Point &startPos = getAncestor<PushObjectMachine>()->startPos;
01387         Point &objPos = getAncestor<PushObjectMachine>()->objPos;
01388         AngTwoPi &acquireOri = getAncestor<PushObjectMachine>()->acquireOri;
01389         NavigationPlan &objToDestPlan = getAncestor<PushObjectMachine>()->objToDestPlan;
01390           
01391         //std::cout << "--------ChoosePathInitToObj" << std::endl;
01392           objToDestPlan.clear();
01393           objToDestPlan.path = plan.path;
01394           
01395           Point vec = objPos-startPos;
01396           AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
01397           float d = vec.xyNorm();
01398           float oriDiff = std::abs(AngSignPi(ori-acquireOri));
01399           //std::cout << "d: " << d << ", oriDiff:" << oriDiff << ", ori: " << ori << ", acquireOri: " << acquireOri << std::endl;
01400           if (d < backupDist*1.2 && oriDiff < M_PI/180*45) {
01401             //FetchObj
01402             std::cout << "Use simple way (straight line) from init to obj" << std::endl;
01403             postStateSuccess();
01404           }
01405           else {
01406             //plan path
01407             std::cout << "Use complex way (need path planning) from init to obj" << std::endl;
01408             postStateFailure();
01409           }
01410       }
01411     };
01412     
01413     /*$nodeclass ChoosePathObjToDest : StateNode : doStart {
01414       //$reference PushObjectMachine::endPos, PushObjectMachine::backupOri;
01415       
01416     std::cout << "--------ChoosePathObjToDest" << std::endl;
01417       Point curPos = VRmixin::theAgent->getCentroid();
01418       //AngTwoPi curHead = VRmixin::theAgent->getOrientation();
01419       Point vec = endPos-curPos;
01420       AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
01421       float d = vec.xyNorm();
01422       float oriDiff = std::abs(AngSignPi(ori-backupOri));
01423       std::cout << "d: " << d << ", oriDiff:" << oriDiff << ", ori: " << ori << ", backupOri: " << backupOri << std::endl;
01424       if (d < backupDist*1.05 && oriDiff < M_PI/180*30) {
01425         //AdjustPush
01426         std::cout << "Use simple way (straight line) from obj to dest" << std::endl;
01427         postStateSuccess();
01428       }
01429       else {
01430         //plan path
01431         std::cout << "Use complex way (need path planning) from obj to dest" << std::endl;
01432         postStateFailure();
01433       }
01434     }*/
01435     
01436     class ChoosePathObjToDest : public StateNode {
01437      public:
01438       ChoosePathObjToDest(const std::string &nodename="ChoosePathObjToDest") : StateNode(nodename) {}
01439       virtual void doStart() {
01440         NavigationPlan &plan = getAncestor<Pilot>()->plan;
01441         Point &endPos = getAncestor<PushObjectMachine>()->endPos;
01442         Point &backupPt = getAncestor<PushObjectMachine>()->backupPt;
01443         AngTwoPi &backupOri = getAncestor<PushObjectMachine>()->backupOri;
01444         NavigationPlan &initToObjPlan = getAncestor<PushObjectMachine>()->initToObjPlan;
01445           
01446         //std::cout << "--------ChoosePathObjToDest" << std::endl;
01447           initToObjPlan.clear();
01448           initToObjPlan.path = plan.path;
01449           initToObjPlan.steps = plan.steps;
01450           
01451           Point curPos = VRmixin::theAgent->getCentroid();
01452           //AngTwoPi curHead = VRmixin::theAgent->getOrientation();
01453           Point vec = endPos-curPos;
01454           AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
01455           Point temp = VRmixin::pilot->curReq->objectShape->getCentroid();
01456           VRmixin::pilot->curReq->objectShape->setPosition(endPos);
01457           //std::cout << "tempBackupOri: " << ori << std::endl;
01458           backupPt = FindFreeSpace(VRmixin::pilot->curReq->objectShape, backupDist, obstDist, ori);
01459           VRmixin::pilot->curReq->objectShape->setPosition(temp);
01460           ReferenceFrameType_t t = backupPt.getRefFrameType();
01461           if (t == unspecified) {
01462             std::cout << "Not enough space at destination." << std::endl;
01463             //postStateFailure();
01464             return;
01465           }
01466           Point vec2 = endPos - backupPt;
01467           backupOri = AngTwoPi(std::atan2(vec2.coordY(), vec2.coordX()));
01468           //std::cout << "backupPt: " << backupPt << ", backupOri: " << backupOri << std::endl;
01469           float d = vec.xyNorm();
01470           float oriDiff = std::abs(AngSignPi(ori-backupOri));
01471           //std::cout << "d: " << d << ", oriDiff:" << oriDiff << ", ori: " << ori << ", backupOri: " << backupOri << std::endl;
01472           if (d < backupDist*1.2 && oriDiff < M_PI/180*45) {
01473             //AdjustPush
01474             std::cout << "Use simple way (straight line) from obj to dest" << std::endl;
01475             postStateSuccess();
01476           }
01477           else {
01478             //plan path
01479             std::cout << "Use complex way (need path planning) from obj to dest" << std::endl;
01480             postStateFailure();
01481           }
01482       }
01483     };
01484     
01485     class SetUpExecute2 : public StateNode {
01486      public:
01487       SetUpExecute2(const std::string &nodename="SetUpExecute2") : StateNode(nodename) {}
01488       virtual void doStart() {
01489         NavigationPlan &plan = getAncestor<Pilot>()->plan;
01490         Point &initPos = getAncestor<Pilot>()->initPos;
01491         AngTwoPi &initHead = getAncestor<Pilot>()->initHead;
01492         PilotRequest &planReq = getAncestor<Pilot>()->planReq;
01493         AngTwoPi &maxTurn = getAncestor<Pilot>()->maxTurn;
01494         bool &pushingObj = getAncestor<Pilot>()->pushingObj;
01495         NavigationPlan &objToDestPlan = getAncestor<PushObjectMachine>()->objToDestPlan;
01496     
01497         //std::cout << "--------SetUpExecute2" << std::endl;
01498           plan.clear();
01499           plan.path = objToDestPlan.path;
01500     
01501           initPos = VRmixin::theAgent->getCentroid();
01502           initHead = VRmixin::theAgent->getOrientation();
01503           PilotRequest &req = *VRmixin::pilot->curReq;
01504           req.turnSpeed = pushTurnSpeed;
01505           req.forwardSpeed = pushForwardSpeed;
01506           planReq = req;
01507           maxTurn = 0.f;
01508           pushingObj = true;
01509           req.collisionAction = PilotTypes::collisionIgnore;
01510           planReq.displayPath = false;
01511       }
01512     };
01513     
01514     class DisplayStraightLineInitToObj : public StateNode {
01515      public:
01516       DisplayStraightLineInitToObj(const std::string &nodename="DisplayStraightLineInitToObj") : StateNode(nodename) {}
01517       virtual void doStart() {
01518         Point &objPos = getAncestor<PushObjectMachine>()->objPos;
01519         Point &startPos = getAncestor<PushObjectMachine>()->startPos;
01520           
01521         //std::cout << "--------DisplayStraightLineInitToObj" << std::endl;
01522           // Display path (straight line) from init to obj
01523           NEW_SHAPE(plannedInitToObjPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
01524           GraphicsData::xyPair U, V;
01525           U.first = startPos.coordX(); U.second = startPos.coordY();
01526           V.first = objPos.coordX(); V.second = objPos.coordY();
01527           plannedInitToObjPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
01528           //NEW_SHAPE(plannedInitToObjPath2, LineData, new LineData(VRmixin::worldShS, startPos, objPos));
01529           //plannedInitToObjPath2->setColor("red");
01530       }
01531     };
01532     
01533     class DisplayStraightLineObjToDest : public StateNode {
01534      public:
01535       DisplayStraightLineObjToDest(const std::string &nodename="DisplayStraightLineObjToDest") : StateNode(nodename) {}
01536       virtual void doStart() {
01537         Point &objPos = getAncestor<PushObjectMachine>()->objPos;
01538         Point &endPos = getAncestor<PushObjectMachine>()->endPos;
01539           
01540         //std::cout << "--------DisplayStraightLineObjToDest" << std::endl;
01541           // Display path (straight line) from obj to dest
01542           NEW_SHAPE(plannedObjToDestPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
01543           GraphicsData::xyPair U, V;
01544           U.first = objPos.coordX(); U.second = objPos.coordY();
01545           V.first = endPos.coordX(); V.second = endPos.coordY();
01546           plannedObjToDestPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
01547           //NEW_SHAPE(plannedObjToDestPath2, LineData, new LineData(VRmixin::worldShS, objPos, endPos));
01548           //plannedObjToDestPath2->setColor("red");
01549       }
01550     };
01551     
01552     class DisplayPathInitToObj : public StateNode {
01553      public:
01554       DisplayPathInitToObj(const std::string &nodename="DisplayPathInitToObj") : StateNode(nodename) {}
01555       virtual void doStart() {
01556         NavigationPlan &plan = getAncestor<Pilot>()->plan;
01557         Point &objPos = getAncestor<PushObjectMachine>()->objPos;
01558           
01559           //std::cout << "--------DisplayPathInitToObj" << std::endl;
01560           // Display path from init to obj
01561           NEW_SHAPE(plannedInitToObjPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
01562           GraphicsData::xyPair U, V;
01563           for ( size_t i = 1; i < plan.path.size(); i++ ) {
01564       U.first = plan.path[i-1].x; U.second = plan.path[i-1].y;
01565       V.first = plan.path[i].x; V.second = plan.path[i].y;
01566       plannedInitToObjPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
01567           }
01568           U.first = plan.path.back().x; U.second = plan.path.back().y;
01569           V.first = objPos.coordX(); V.second = objPos.coordY();
01570           plannedInitToObjPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
01571       }
01572     };
01573     
01574     class DisplayPathObjToDest : public StateNode {
01575      public:
01576       DisplayPathObjToDest(const std::string &nodename="DisplayPathObjToDest") : StateNode(nodename) {}
01577       virtual void doStart() {
01578         NavigationPlan &plan = getAncestor<Pilot>()->plan;
01579         Point &endPos = getAncestor<PushObjectMachine>()->endPos;
01580           
01581         //std::cout << "--------DisplayPathObjToDest" << std::endl;
01582           // Display path from obj to dest
01583           NEW_SHAPE(plannedObjToDestPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
01584           GraphicsData::xyPair U, V;
01585           for ( size_t i = 1; i < plan.path.size(); i++ ) {
01586       U.first = plan.path[i-1].x; U.second = plan.path[i-1].y;
01587       V.first = plan.path[i].x; V.second = plan.path[i].y;
01588       plannedObjToDestPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
01589           }
01590           U.first = plan.path.back().x; U.second = plan.path.back().y;
01591           V.first = endPos.coordX(); V.second = endPos.coordY();
01592           plannedObjToDestPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
01593       }
01594     };
01595 
01596     class WalkBackward : public WalkNode {
01597      public:
01598       WalkBackward(const std::string &nodename="WalkBackward") : WalkNode(nodename) {}
01599       virtual void doStart() {
01600         //std::cout << "--------WalkBackward" << std::endl;
01601           MMAccessor<WalkMC> walk_acc(getMC());
01602           getMC()->setTargetDisplacement(-backupDist+robotDiam,0,0,0);
01603           motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
01604       }
01605     };
01606     
01607     class WalkForward : public WalkNode {
01608      public:
01609       WalkForward(const std::string &nodename="WalkForward") : WalkNode(nodename) {}
01610       virtual void doStart() {
01611         //std::cout << "--------WalkForward" << std::endl;
01612           getMC()->setTargetDisplacement(backupDist-robotDiam,0,0,0);
01613           motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);        
01614       }
01615     };
01616     
01617     class FetchObj : public WalkNode {
01618      public:
01619       FetchObj(const std::string &nodename="FetchObj") : WalkNode(nodename) {}
01620       virtual void doStart() {
01621         Point &objPos = getAncestor<PushObjectMachine>()->objPos;
01622           
01623           std::cout << "--------Fetch the object" << std::endl;
01624           std::cout << "curPos: " << VRmixin::theAgent->getCentroid() << ", curHead: "
01625         << VRmixin::theAgent->getOrientation() << std::endl;
01626           PilotRequest &preq = *VRmixin::pilot->curReq;
01627           preq.collisionAction = PilotTypes::collisionIgnore;
01628           
01629           AngSignPi turnAng;
01630           float dist;
01631           if ( preq.objectMatcher == NULL ) {
01632             //std::cout << "No objectMatcher specified" << std::endl;
01633       Point curPos = VRmixin::theAgent->getCentroid();
01634             AngTwoPi curHead = VRmixin::theAgent->getOrientation();
01635             Point vec = objPos - curPos;
01636             float x = vec.coordX(), y = vec.coordY();
01637             turnAng = AngSignPi(std::atan2(y, x)) - curHead;
01638             dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
01639           } else {
01640       ShapeRoot localObj = (*preq.objectMatcher)(preq.objectShape);
01641       if ( ! localObj.isValid() ) {
01642         std::cout << "Can't find the object" << std::endl;
01643         Point curPos = VRmixin::theAgent->getCentroid();
01644               AngTwoPi curHead = VRmixin::theAgent->getOrientation();
01645               Point vec = objPos - curPos;
01646               float x = vec.coordX(), y = vec.coordY();
01647               turnAng = AngSignPi(std::atan2(y, x)) - curHead;
01648               dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
01649         //postStateFailure();
01650         //return;
01651       } else {
01652         Point ctPt = localObj->getCentroid();
01653               float x = ctPt.coordX(), y = ctPt.coordY();
01654               turnAng = AngSignPi(std::atan2(y, x));
01655               dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
01656             }
01657           }
01658           
01659           std::cout << "Turn " << float(turnAng)/M_PI*180.f << " degree" << std::endl;
01660           getMC()->setTargetDisplacement(0, 0, turnAng, 0);
01661           std::cout << "Walk forward " << dist << " mm" << std::endl;
01662           // wp_acc->addEgocentricWaypoint(dist, 0, 0, true, pushForwardSpeed, 0.f);
01663           motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
01664       }
01665     };
01666     
01667     class AdjustPush : public WaypointWalkNode {
01668      public:
01669       AdjustPush(const std::string &nodename="AdjustPush") : WaypointWalkNode(nodename) {}
01670       virtual void doStart() {
01671         Point &endPos = getAncestor<PushObjectMachine>()->endPos;
01672           
01673           std::cout << "--------Adjust before pushing finish" << std::endl;
01674           std::cout << "curPos: " << VRmixin::theAgent->getCentroid() << ", curHead: "
01675         << VRmixin::theAgent->getOrientation() << std::endl;
01676           PilotRequest &req = *VRmixin::pilot->curReq;
01677           req.collisionAction = PilotTypes::collisionIgnore;
01678           
01679           Point curPos = VRmixin::theAgent->getCentroid();
01680           AngTwoPi curHead = VRmixin::theAgent->getOrientation();
01681           Point vec = endPos - curPos;
01682           float x = vec.coordX(), y = vec.coordY();
01683           AngSignPi turnAng = AngSignPi(std::atan2(y, x)) - AngSignPi(curHead);
01684           float dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
01685           
01686           MMAccessor<WaypointWalkMC> wp_acc(getMC());
01687           wp_acc->getWaypointList().clear();
01688     #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
01689           std::cout << "Turn " << float(turnAng)/M_PI*180.f << " degree" << std::endl;
01690           //std::cout << "Turn " << turnAng << " radians" << std::endl;
01691           wp_acc->addEgocentricWaypoint(0, 0, turnAng, true, 0.f, pushTurnSpeed);
01692           std::cout << "Walk forward " << dist << " mm" << std::endl;
01693           wp_acc->addEgocentricWaypoint(dist, 0, 0, true, pushForwardSpeed, 0.f);
01694     #else
01695           std::cout << "Turn " << float(turnAng)/M_PI*180.f << " degree" << std::endl;
01696           //std::cout << "Turn " << turnAng << " radians" << std::endl;
01697           wp_acc->addEgocentricWaypoint(0, 0, turnAng, true, 0.f, pushTurnSpeed);
01698           std::cout << "Walk forward " << dist << " mm" << std::endl;
01699           wp_acc->addEgocentricWaypoint(dist, 0, 0, true, pushForwardSpeed, 0.f);
01700     #endif
01701           motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
01702       }
01703     };
01704 
01705     class CheckObjectPos : public StateNode {
01706      public:
01707       CheckObjectPos(const std::string &nodename="CheckObjectPos") : StateNode(nodename) {}
01708       virtual void doStart() {
01709         Point &endPos = getAncestor<PushObjectMachine>()->endPos;
01710         bool &isObs = getAncestor<PushObjectMachine>()->isObs;
01711           
01712         //std::cout << "--------CheckObjectPos" << std::endl;
01713           PilotRequest &req = *VRmixin::pilot->curReq;
01714           if ( req.objectMatcher == NULL ) {
01715       //std::cout << "No objectMatcher specified" << std::endl;
01716       VRmixin::pilot->curReq->objectShape->setPosition(endPos);
01717       postStateSuccess();
01718           } else {
01719       ShapeRoot localObj = (*req.objectMatcher)(req.objectShape);
01720       if ( ! localObj.isValid() ) {
01721         std::cout << "Can't find the object" << std::endl;
01722         postStateFailure();
01723       } else {
01724         localObj->applyTransform(VRmixin::mapBuilder->localToWorldMatrix, unspecified);
01725         Point pt = Point(localObj->getCentroid().coordX(), localObj->getCentroid().coordY(), 0, allocentric);
01726         std::cout << "end position: " << pt << std::endl;
01727         VRmixin::pilot->curReq->objectShape->setPosition(pt);
01728         VRmixin::pilot->curReq->objectShape->setObstacle(isObs);
01729         if (pt.xyDistanceFrom(endPos) < 100) {
01730           std::cout << "Push finish" << std::endl;
01731           postStateSuccess();
01732         }
01733         else {
01734           std::cout << "To much error on end position" << std::endl;
01735           postStateSuccess();
01736         }
01737       }
01738           }
01739       }
01740     };
01741 
01742     class MoveObjectPos : public StateNode {
01743      public:
01744       MoveObjectPos(const std::string &nodename="MoveObjectPos") : StateNode(nodename) {}
01745       virtual void doStart() {
01746         Point &endPos = getAncestor<PushObjectMachine>()->endPos;
01747     
01748         //std::cout << "--------MoveObjectPos" << std::endl;
01749           //Should using vision first
01750           VRmixin::pilot->curReq->objectShape->setPosition(endPos);
01751       }
01752     };
01753 
01754     virtual void setup() { // set up PushObjectMachine
01755      SetMaxTurn *setmaxturn2 = new SetMaxTurn("setmaxturn2");
01756      addNode(setmaxturn2);
01757 
01758      CheckParameters *checkparams = new CheckParameters("checkparams");
01759      addNode(checkparams);
01760 
01761      ReportCompletion *reportcompletion9 = new ReportCompletion("reportcompletion9",invalidRequest);
01762      addNode(reportcompletion9);
01763 
01764      SetupLandmarkExtractor *localize = new SetupLandmarkExtractor("localize");
01765      addNode(localize);
01766 
01767      LocalizationUtility *locutil = new LocalizationUtility("locutil");
01768      addNode(locutil);
01769 
01770      ClearOldPath *plandelivery = new ClearOldPath("plandelivery");
01771      addNode(plandelivery);
01772 
01773      SetUpObjToDest *setupobjtodest1 = new SetUpObjToDest("setupobjtodest1");
01774      addNode(setupobjtodest1);
01775 
01776      PlanPath *planpath3 = new PlanPath("planpath3");
01777      addNode(planpath3);
01778 
01779      ReportCompletion *reportcompletion10 = new ReportCompletion("reportcompletion10",startCollides);
01780      addNode(reportcompletion10);
01781 
01782      ReportCompletion *reportcompletion11 = new ReportCompletion("reportcompletion11",endCollides);
01783      addNode(reportcompletion11);
01784 
01785      ReportCompletion *reportcompletion12 = new ReportCompletion("reportcompletion12",noPath);
01786      addNode(reportcompletion12);
01787 
01788      ReportCompletion *reportcompletion13 = new ReportCompletion("reportcompletion13");
01789      addNode(reportcompletion13);
01790 
01791      AddBackupPt *plandelivery2 = new AddBackupPt("plandelivery2");
01792      addNode(plandelivery2);
01793 
01794      AddAcquirePt *addacquire = new AddAcquirePt("addacquire");
01795      addNode(addacquire);
01796 
01797      SetUpObjToDest2 *setupobjtodest21 = new SetUpObjToDest2("setupobjtodest21");
01798      addNode(setupobjtodest21);
01799 
01800      ReportCompletion *reportcompletion14 = new ReportCompletion("reportcompletion14",noSpace);
01801      addNode(reportcompletion14);
01802 
01803      ReportCompletion *reportcompletion15 = new ReportCompletion("reportcompletion15",noSpace);
01804      addNode(reportcompletion15);
01805 
01806      PlanPath *planpath2 = new PlanPath("planpath2");
01807      addNode(planpath2);
01808 
01809      ReportCompletion *reportcompletion16 = new ReportCompletion("reportcompletion16",startCollides);
01810      addNode(reportcompletion16);
01811 
01812      ReportCompletion *reportcompletion17 = new ReportCompletion("reportcompletion17",endCollides);
01813      addNode(reportcompletion17);
01814 
01815      ReportCompletion *reportcompletion18 = new ReportCompletion("reportcompletion18",noPath);
01816      addNode(reportcompletion18);
01817 
01818      ReportCompletion *reportcompletion19 = new ReportCompletion("reportcompletion19");
01819      addNode(reportcompletion19);
01820 
01821      ChoosePathInitToObj *planacquire = new ChoosePathInitToObj("planacquire");
01822      addNode(planacquire);
01823 
01824      DisplayStraightLineInitToObj *displaystraightlineinittoobj1 = new DisplayStraightLineInitToObj("displaystraightlineinittoobj1");
01825      addNode(displaystraightlineinittoobj1);
01826 
01827      SetUpInitToObj *setupinittoobj1 = new SetUpInitToObj("setupinittoobj1");
01828      addNode(setupinittoobj1);
01829 
01830      PlanPath *planpath1 = new PlanPath("planpath1");
01831      addNode(planpath1);
01832 
01833      ReportCompletion *reportcompletion20 = new ReportCompletion("reportcompletion20",startCollides);
01834      addNode(reportcompletion20);
01835 
01836      ReportCompletion *reportcompletion21 = new ReportCompletion("reportcompletion21",endCollides);
01837      addNode(reportcompletion21);
01838 
01839      ReportCompletion *reportcompletion22 = new ReportCompletion("reportcompletion22",noPath);
01840      addNode(reportcompletion22);
01841 
01842      ReportCompletion *reportcompletion23 = new ReportCompletion("reportcompletion23");
01843      addNode(reportcompletion23);
01844 
01845      ConstructNavPlan *execute = new ConstructNavPlan("execute");
01846      addNode(execute);
01847 
01848      DisplayPathInitToObj *displaypathinittoobj1 = new DisplayPathInitToObj("displaypathinittoobj1");
01849      addNode(displaypathinittoobj1);
01850 
01851      ExecutePlan *executeplan1 = new ExecutePlan("executeplan1");
01852      addNode(executeplan1);
01853 
01854      LookForObject *lookforobj = new LookForObject("lookforobj");
01855      addNode(lookforobj);
01856 
01857      FetchObj *fetch = new FetchObj("fetch");
01858      addNode(fetch);
01859 
01860      ChoosePathObjToDest *planpush = new ChoosePathObjToDest("planpush");
01861      addNode(planpush);
01862 
01863      DisplayStraightLineObjToDest *displaystraightlineobjtodest1 = new DisplayStraightLineObjToDest("displaystraightlineobjtodest1");
01864      addNode(displaystraightlineobjtodest1);
01865 
01866      SetUpExecute2 *setupexecute21 = new SetUpExecute2("setupexecute21");
01867      addNode(setupexecute21);
01868 
01869      ConstructNavPlan *execute2 = new ConstructNavPlan("execute2");
01870      addNode(execute2);
01871 
01872      DisplayPathObjToDest *displaypathobjtodest1 = new DisplayPathObjToDest("displaypathobjtodest1");
01873      addNode(displaypathobjtodest1);
01874 
01875      ExecutePlan *executeplan2 = new ExecutePlan("executeplan2");
01876      addNode(executeplan2);
01877 
01878      AdjustPush *adjustpush = new AdjustPush("adjustpush");
01879      addNode(adjustpush);
01880 
01881      WalkBackward *walkbackward1 = new WalkBackward("walkbackward1");
01882      addNode(walkbackward1);
01883      walkbackward1->setMC(VRmixin::pilot->waypointwalk_id);
01884 
01885      LookForTarget *lookforobj2 = new LookForTarget("lookforobj2");
01886      addNode(lookforobj2);
01887 
01888      CheckObjectPos *checkobj = new CheckObjectPos("checkobj");
01889      addNode(checkobj);
01890 
01891      ReportCompletion *complete = new ReportCompletion("complete");
01892      addNode(complete);
01893 
01894      startnode = setmaxturn2;
01895 
01896      setmaxturn2->addTransition(new NullTrans("nulltrans13",checkparams));
01897      checkparams->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans26",localize,StateNode::successSignal));
01898      checkparams->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans27",reportcompletion9,StateNode::failureSignal));
01899      localize->addTransition(new NullTrans("nulltrans14",locutil));
01900      locutil->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans28",plandelivery,StateNode::successSignal));
01901      locutil->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans29",plandelivery,StateNode::failureSignal));
01902      plandelivery->addTransition(new NullTrans("nulltrans15",setupobjtodest1));
01903      setupobjtodest1->addTransition(new NullTrans("nulltrans16",planpath3));
01904      planpath3->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans30",reportcompletion10,GenericRRTBase::START_COLLIDES));
01905      planpath3->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans31",reportcompletion11,GenericRRTBase::END_COLLIDES));
01906      planpath3->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans32",reportcompletion12,GenericRRTBase::MAX_ITER));
01907      planpath3->addTransition(new CompletionTrans("completiontrans17",plandelivery2));
01908      planpath3->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans33",reportcompletion13,StateNode::failureSignal));
01909      plandelivery2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans34",addacquire,StateNode::successSignal));
01910      addacquire->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans35",setupobjtodest21,StateNode::successSignal));
01911      setupobjtodest21->addTransition(new NullTrans("nulltrans17",planpath2));
01912      plandelivery2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans36",reportcompletion14,StateNode::failureSignal));
01913      addacquire->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans37",reportcompletion15,StateNode::failureSignal));
01914      planpath2->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans38",reportcompletion16,GenericRRTBase::START_COLLIDES));
01915      planpath2->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans39",reportcompletion17,GenericRRTBase::END_COLLIDES));
01916      planpath2->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans40",reportcompletion18,GenericRRTBase::MAX_ITER));
01917      planpath2->addTransition(new CompletionTrans("completiontrans18",planacquire));
01918      planpath2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans41",reportcompletion19,StateNode::failureSignal));
01919      planacquire->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans42",displaystraightlineinittoobj1,StateNode::successSignal));
01920      displaystraightlineinittoobj1->addTransition(new NullTrans("nulltrans18",lookforobj));
01921      planacquire->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans43",setupinittoobj1,StateNode::failureSignal));
01922      setupinittoobj1->addTransition(new NullTrans("nulltrans19",planpath1));
01923      planpath1->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans44",reportcompletion20,GenericRRTBase::START_COLLIDES));
01924      planpath1->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans45",reportcompletion21,GenericRRTBase::END_COLLIDES));
01925      planpath1->addTransition(new SignalTrans<GenericRRTBase::PlanPathResultCode>("signaltrans46",reportcompletion22,GenericRRTBase::MAX_ITER));
01926      planpath1->addTransition(new CompletionTrans("completiontrans19",execute));
01927      planpath1->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans47",reportcompletion23,StateNode::failureSignal));
01928      execute->addTransition(new CompletionTrans("completiontrans20",displaypathinittoobj1));
01929      displaypathinittoobj1->addTransition(new NullTrans("nulltrans20",executeplan1));
01930      executeplan1->addTransition(new CompletionTrans("completiontrans21",lookforobj));
01931      lookforobj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans48",fetch,StateNode::failureSignal));
01932      lookforobj->addTransition(new CompletionTrans("completiontrans22",fetch));
01933      fetch->addTransition(new CompletionTrans("completiontrans23",planpush));
01934      planpush->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans49",displaystraightlineobjtodest1,StateNode::successSignal));
01935      displaystraightlineobjtodest1->addTransition(new NullTrans("nulltrans21",adjustpush));
01936      planpush->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans50",setupexecute21,StateNode::failureSignal));
01937      setupexecute21->addTransition(new NullTrans("nulltrans22",execute2));
01938      execute2->addTransition(new CompletionTrans("completiontrans24",displaypathobjtodest1));
01939      displaypathobjtodest1->addTransition(new NullTrans("nulltrans23",executeplan2));
01940      executeplan2->addTransition(new CompletionTrans("completiontrans25",adjustpush));
01941      adjustpush->addTransition(new CompletionTrans("completiontrans26",walkbackward1));
01942      walkbackward1->addTransition(new CompletionTrans("completiontrans27",lookforobj2));
01943      lookforobj2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans51",checkobj,StateNode::failureSignal));
01944      lookforobj2->addTransition(new CompletionTrans("completiontrans28",checkobj));
01945      checkobj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans52",complete,StateNode::successSignal));
01946      checkobj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans53",complete,StateNode::failureSignal));
01947     }
01948 
01949     NavigationPlan initToObjPlan;   // from $provide
01950     NavigationPlan objToDestPlan;   // from $provide
01951     Point startPos;   // from $provide
01952     Point objPos;   // from $provide
01953     Point endPos;   // from $provide
01954     AngTwoPi startOri;   // from $provide
01955     AngTwoPi acquireOri;   // from $provide
01956     AngTwoPi backupOri;   // from $provide
01957     Point backupPt;   // from $provide
01958     Point acquirePt;   // from $provide
01959     bool useSimpleWayObjToDest;   // from $provide
01960     bool useSimpleWayInitToObj;   // from $provide
01961     bool isObs;   // from $provide
01962   };
01963 
01964   //================ VisualSearchMachine ================
01965 
01966   class VisualSearchMachine : public StateNode {
01967    public:
01968     VisualSearchMachine(const std::string &nodename="VisualSearchMachine") : StateNode(nodename) {}
01969 
01970     class Search : public MapBuilderNode {
01971      public:
01972       Search(const std::string &nodename="Search") : MapBuilderNode(nodename) {}
01973       virtual void doStart() {
01974           if (VRmixin::pilot->curReq->searchObjectExtractor == NULL) {
01975       cout << "Pilot received visualSearch request with invalid searchObjectExtractor field" << endl;
01976       VRmixin::pilot->requestComplete(invalidRequest);
01977           }
01978           else if (VRmixin::pilot->curReq->searchExitTest == NULL) {
01979       cout << "Pilot received visualSearch request with no exitTest" << endl;
01980       VRmixin::pilot->requestComplete(invalidRequest);
01981           }
01982           mapreq = *VRmixin::pilot->curReq->searchObjectExtractor;
01983       }
01984     };
01985 
01986     class Check : public StateNode {
01987      public:
01988       Check(const std::string &nodename="Check") : StateNode(nodename) {}
01989       virtual void doStart() {
01990           if (VRmixin::pilot->curReq->searchExitTest())
01991       VRmixin::pilot->requestComplete(noError);
01992           else if (VRmixin::pilot->curReq->searchRotationAngle == 0)
01993       VRmixin::pilot->requestComplete(searchFailed);
01994           else
01995       postStateCompletion();
01996       }
01997     };
01998 
01999     class Rotate : public WalkNode {
02000      public:
02001       Rotate(const std::string &nodename="Rotate") : WalkNode(nodename) {}
02002       virtual void doStart() {
02003           getMC()->setTargetDisplacement(0,0,VRmixin::pilot->curReq->searchRotationAngle,0);
02004       }
02005     };
02006 
02007     virtual void setup() {
02008      startnode = new Search("startnode");
02009      addNode(startnode);
02010 
02011      Check *check1 = new Check("check1");
02012      addNode(check1);
02013 
02014      Rotate *rotate1 = new Rotate("rotate1");
02015      addNode(rotate1);
02016      rotate1->setMC(VRmixin::pilot->walk_id);
02017 
02018      startnode->addTransition(new EventTrans("eventtrans3",check1,EventBase::mapbuilderEGID,(size_t)startnode,EventBase::statusETID));
02019      check1->addTransition(new CompletionTrans("completiontrans29",rotate1));
02020      rotate1->addTransition(new CompletionTrans("completiontrans30",startnode));
02021     }
02022   };
02023 
02024 
02025   class SetOdometryMachine : public StateNode {
02026    public:
02027     SetOdometryMachine(const std::string &nodename="SetOdometryMachine") : StateNode(nodename), turn1_(NULL), turn2_(NULL) {}
02028 
02029     class TurnHead : public HeadPointerNode, public CurrVisualOdometry {
02030      public:
02031       TurnHead(const std::string &nodename, float _angle) : HeadPointerNode(nodename), CurrVisualOdometry(nodename), angle(_angle) {}
02032       float angle;  // cache the constructor's parameter
02033       
02034         virtual void preStart() {
02035     HeadPointerNode::preStart();
02036    #ifdef TGT_HAS_HEAD
02037     getMC()->setMaxSpeed(RobotInfo::PanOffset, 1.5f);
02038    #endif
02039         }
02040   
02041         virtual void doStart() {
02042     HeadPointerNode::doStart();
02043     update(true);
02044    #ifdef TGT_HAS_HEAD
02045     getMC()->setJointValue(RobotInfo::PanOffset, mathutils::deg2rad(angle));
02046    #endif
02047         }
02048   
02049         virtual void doEvent() {
02050     HeadPointerNode::doEvent();
02051     postStateCompletion();
02052         }
02053   
02054         virtual void doStop() {
02055     update(true);
02056     std::cout << "Translation: " << getTranslation() << std::endl;
02057         }
02058     };
02059 
02060     TurnHead *turn1_;
02061     TurnHead *turn2_;
02062 
02063     virtual void doStop() {
02064       // CreateMotionModel<LocalizationParticle> *model = 
02065       //   (CreateMotionModel<LocalizationParticle>*)VRmixin::particleFilter->getMotionModel();
02066       // model->getOdo().setConversionParameters(turn1_->getTranslation()/15.0f, 0.0f);
02067       if ( VRmixin::imageOdometry != NULL ) {
02068   std::cout << "Setting visual odometry parameters" << std::endl;
02069   VRmixin::imageOdometry->setConversionParameters(turn1_->getTranslation()/5.0f, 0.0f);
02070       }
02071     }
02072 
02073     virtual void setup() {
02074      TurnHead *turn1 = new TurnHead("turn1",5.0f);
02075      addNode(turn1);
02076 
02077      TurnHead *turn2 = new TurnHead("turn2",0.0f);
02078      addNode(turn2);
02079 
02080      ReportCompletion *reportcompletion24 = new ReportCompletion("reportcompletion24");
02081      addNode(reportcompletion24);
02082 
02083      startnode = turn1;
02084 
02085      turn1->addTransition(new TimeOutTrans("timeouttrans3",turn2,2000));
02086      turn2->addTransition(new TimeOutTrans("timeouttrans4",reportcompletion24,2000));
02087 
02088       turn1_ = turn1;
02089       turn2_ = turn2;
02090     }
02091    private:  // dummy copy and assignment to suppress compiler warnings
02092     SetOdometryMachine(const SetOdometryMachine&);
02093     SetOdometryMachine& operator=(const SetOdometryMachine&);
02094   };
02095 
02096 
02097   //================ Dispatch ================
02098   // All the Pilot's little state machines are children of the
02099   // Dispatch node, so when a call to Dispatch::finish() causes it to
02100   // complete and exit, everything gets shut down.
02101 
02102   class Dispatch : public StateNode {
02103    public:
02104     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) {}
02105 
02106     StateNode *localizeMachine_, *walkMachine_, *waypointWalkMachine_, *setVelocityMachine_, 
02107       *goToShapeMachine_, *pushObjectMachine_, *visualSearchMachine_, *setOdometryMachine_,
02108       *reportSuccess_;
02109 
02110     void finish() {
02111       postStateCompletion();
02112     }
02113 
02114     virtual void doStart() {
02115       PilotTypes::RequestType_t reqType = VRmixin::pilot->curReq->getRequestType();
02116       switch ( reqType) {
02117       case localize:
02118         localizeMachine_->start();
02119   break;
02120 
02121       case walk:
02122   walkMachine_->start();
02123   break;
02124 
02125       case waypointWalk:
02126   waypointWalkMachine_->start();
02127   break;
02128 
02129       case setVelocity:
02130   setVelocityMachine_->start();
02131   break;
02132 
02133       case goToShape:
02134   goToShapeMachine_->start();
02135   break;
02136 
02137       case pushObject:
02138   pushObjectMachine_->start();
02139   break;
02140 
02141       case visualSearch:
02142   visualSearchMachine_->start();
02143   break;
02144 
02145       case setOdometry:
02146   setOdometryMachine_->start();
02147   break;
02148 
02149       case noRequest:
02150         reportSuccess_->start();
02151         break;
02152 
02153       default:
02154   cout << "Illegal Pilot request type: " << reqType << endl;
02155   VRmixin::pilot->requestComplete(invalidRequest);
02156       }
02157     }
02158 
02159     virtual void setup() {
02160      StateNode *dispatchDummyStart = new StateNode("dispatchDummyStart");
02161      addNode(dispatchDummyStart);
02162 
02163      ReportCompletion *reportSuccess = new ReportCompletion("reportSuccess");
02164      addNode(reportSuccess);
02165 
02166      ReportCompletion *reportFailure = new ReportCompletion("reportFailure",someError);
02167      addNode(reportFailure);
02168 
02169      LocalizationMachine *localizemachine = new LocalizationMachine("localizemachine");
02170      addNode(localizemachine);
02171 
02172      ReportCompletion *reportcompletion25 = new ReportCompletion("reportcompletion25",cantLocalize);
02173      addNode(reportcompletion25);
02174 
02175      WalkMachine *walkmachine = new WalkMachine("walkmachine");
02176      addNode(walkmachine);
02177 
02178      WaypointWalkMachine *waypointwalkmachine = new WaypointWalkMachine("waypointwalkmachine");
02179      addNode(waypointwalkmachine);
02180 
02181      SetVelocityMachine *setvelocitymachine = new SetVelocityMachine("setvelocitymachine");
02182      addNode(setvelocitymachine);
02183 
02184      GoToShapeMachine *gotoshapemachine = new GoToShapeMachine("gotoshapemachine");
02185      addNode(gotoshapemachine);
02186 
02187      PushObjectMachine *pushobjectmachine = new PushObjectMachine("pushobjectmachine");
02188      addNode(pushobjectmachine);
02189 
02190      VisualSearchMachine *visualsearchmachine = new VisualSearchMachine("visualsearchmachine");
02191      addNode(visualsearchmachine);
02192 
02193      SetOdometryMachine *setOdometryMachine = new SetOdometryMachine("setOdometryMachine");
02194      addNode(setOdometryMachine);
02195 
02196      startnode = dispatchDummyStart;
02197 
02198      localizemachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans54",reportSuccess,StateNode::successSignal));
02199      localizemachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans55",reportcompletion25,StateNode::failureSignal));
02200      waypointwalkmachine->addTransition(new CompletionTrans("completiontrans31",reportSuccess));
02201      waypointwalkmachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans56",reportFailure,StateNode::failureSignal));
02202      gotoshapemachine->addTransition(new CompletionTrans("completiontrans32",reportSuccess));
02203      gotoshapemachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans57",reportFailure,StateNode::failureSignal));
02204      pushobjectmachine->addTransition(new CompletionTrans("completiontrans33",reportSuccess));
02205      pushobjectmachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans58",reportFailure,StateNode::failureSignal));
02206      visualsearchmachine->addTransition(new CompletionTrans("completiontrans34",reportSuccess));
02207      visualsearchmachine->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans59",reportFailure,StateNode::failureSignal));
02208      setOdometryMachine->addTransition(new CompletionTrans("completiontrans35",reportSuccess));
02209 
02210       startnode = NULL; // keeps the dummy node out of the Event Logger trace
02211       // Save these statenode pointers for use by dispatch,
02212       localizeMachine_ = localizemachine;
02213       walkMachine_ = walkmachine;
02214       waypointWalkMachine_ = waypointwalkmachine;
02215       setVelocityMachine_ = setvelocitymachine;
02216       goToShapeMachine_ = gotoshapemachine;
02217       pushObjectMachine_ = pushobjectmachine;
02218       visualSearchMachine_ = visualsearchmachine;
02219       setOdometryMachine_ = setOdometryMachine;
02220       reportSuccess_ = reportSuccess;
02221     }
02222    private:  // dummy copy and assignment to suppress compiler warnings
02223     Dispatch(const Dispatch&);
02224     Dispatch& operator=(const Dispatch&);
02225   };
02226 
02227   virtual void setup() {   // Pilot's setup
02228     SharedObject<WaypointWalkMC> waypointwalk_mc;
02229     SharedObject<WalkMC> walk_mc;
02230 #ifdef PILOT_USES_SHARED_MOTION_COMMANDS
02231     walk_id = motman->addPersistentMotion(walk_mc, MotionManager::kIgnoredPriority);
02232     waypointwalk_id = motman->addPersistentMotion(waypointwalk_mc, MotionManager::kIgnoredPriority);
02233 #endif
02234    StateNode *pilotWaitForDispatch = new StateNode("pilotWaitForDispatch");
02235    addNode(pilotWaitForDispatch);
02236 
02237    Dispatch *dispatch = new Dispatch("dispatch");
02238    addNode(dispatch);
02239 
02240    startnode = pilotWaitForDispatch;
02241 
02242    CompletionTrans *pilot_dispatch = new CompletionTrans("pilot_dispatch",pilotWaitForDispatch);
02243    dispatch->addTransition(pilot_dispatch);
02244 
02245     VRmixin::pilot->dispatch_ = dispatch;
02246   }
02247 
02248 private:
02249   Pilot(const Pilot&); //!< copy constructor; do not call
02250   Pilot& operator=(const Pilot&); //!< assignment operator; do not call
02251 
02252   static PilotVerbosity_t verbosity;
02253 
02254   std::queue<PilotRequest*> requests;   //!< Queue of Pilot requests
02255   PilotRequest *curReq;
02256   unsigned int idCounter;   //!< Pilot request counter for assigning a unique id
02257   MotionManager::MC_ID walk_id;
02258   MotionManager::MC_ID waypointwalk_id;
02259   Dispatch *dispatch_;
02260 
02261   /*! @endcond */
02262 
02263   std::vector<ShapeRoot> defaultLandmarks;   // from $provide
02264   MapBuilderRequest* defaultLandmarkExtractor;   // from $provide
02265   Point initPos;   // from $provide
02266   AngTwoPi initHead;   // from $provide
02267   PilotRequest planReq;   // from $provide
02268   NavigationPlan plan;   // from $provide
02269   AngTwoPi maxTurn;   // from $provide
02270   bool pushingObj;   // from $provide
02271 };
02272 
02273 std::ostream& operator<<(std::ostream& os, const NavigationStep &step);
02274 std::ostream& operator<<(std::ostream& os, const NavigationPlan &plan);
02275 
02276 } // namespace
02277 
02278 #endif

DualCoding 5.1CVS
Generated Mon May 9 04:56:27 2016 by Doxygen 1.6.3