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 #include <queue>
00006 
00007 #include "Behaviors/StateNode.h"
00008 #include "Behaviors/Nodes/PostureNode.h"
00009 #include "Behaviors/Nodes/WaypointWalkNode.h"
00010 #include "Motion/MotionManager.h"
00011 
00012 #include "PilotRequest.h"
00013 #include "VisualRoutinesStateNode.h"
00014 
00015 namespace DualCoding {
00016 
00017 class Pilot : public StateNode {
00018 public:
00019 
00020   typedef unsigned int PilotVerbosity_t;
00021   static const PilotVerbosity_t PVstart = 1<<0;
00022   static const PilotVerbosity_t PVevents = 1<<1;
00023   static const PilotVerbosity_t PVexecute = 1<<2;
00024   static const PilotVerbosity_t PVsuccess = 1<<3;
00025   static const PilotVerbosity_t PVfailure = 1<<4;
00026   static const PilotVerbosity_t PVcomplete = 1<<5;
00027 
00028 private:
00029   static PilotVerbosity_t verbosity;
00030 public:
00031   static void setVerbosity(PilotVerbosity_t v) { verbosity = v; }
00032   static PilotVerbosity_t getVerbosity() { return verbosity; }
00033 
00034   class Success : public StateNode {
00035   public:
00036     Success() : StateNode("Success") {}
00037     virtual void DoStart();
00038   };
00039 
00040   class Failure : public StateNode {
00041   public:
00042     Failure() : StateNode("Failure") {}
00043     virtual void DoStart();
00044   };
00045 
00046   class Walk : public WaypointWalkNode {
00047   public:
00048     Walk() : WaypointWalkNode("Walk") {}
00049     virtual void DoStart();
00050   };
00051 
00052   class VisualSearch : public VisualRoutinesStateNode {
00053   public:
00054     VisualSearch() : VisualRoutinesStateNode("VisualSearch"), mapbuilder_id(0) {}
00055     virtual void DoStart();
00056     unsigned int mapbuilder_id;
00057   };
00058 
00059   class VisualSearchHaveMap : public VisualRoutinesStateNode {
00060   public:
00061     VisualSearchHaveMap() : VisualRoutinesStateNode("VisualSearchHaveMap") {}
00062     virtual void DoStart();
00063   };
00064 
00065   class VisualSearchWalk : public WaypointWalkNode {
00066   public:
00067     VisualSearchWalk() : WaypointWalkNode("VisualSearchWalk"), walk_id(MotionManager::invalid_MC_ID) {}
00068     virtual void DoStart();
00069     MotionManager::MC_ID walk_id;
00070   };
00071 
00072   class GotoShape : public WaypointWalkNode {
00073   public:
00074     GotoShape() : WaypointWalkNode("GotoShape") {}
00075     virtual void DoStart();
00076   };
00077 
00078   class BuildTarget : public VisualRoutinesStateNode {
00079   public:
00080     BuildTarget(bool _buildFrontLeft = true, bool _buildFrontRight = true, bool _buildBackLeft = true, bool _buildBackRight = true, bool _lookAtCentroid = true, int _maxRetries = 10) :
00081                VisualRoutinesStateNode("BuildTarget"),
00082                buildFrontLeft(_buildFrontLeft), buildFrontRight(_buildFrontRight), buildBackLeft(_buildBackLeft), buildBackRight(_buildBackRight), lookAtCentroid(_lookAtCentroid),
00083                frontLeftPt(0, 0, 0), frontRightPt(0, 0, 0), backLeftPt(0, 0, 0), backRightPt(0, 0, 0), centroid(0, 0, 0),
00084                maxRetries(_maxRetries), triesLeft(0),
00085                myState(none) {}
00086     virtual void DoStart();
00087     virtual void DoStop();
00088     virtual void processEvent(const EventBase& event);
00089     
00090     void setParams(bool _buildFrontLeft = true, bool _buildFrontRight = true, bool _buildBackLeft = true, bool _buildBackRight = true, bool _lookAtCentroid = true, int _maxRetries = 10);
00091   
00092   protected:
00093     void generateMapBuilderRequest();
00094     void keepBestTarget();
00095     bool getNextPoint(Point *point);
00096     
00097     bool buildFrontLeft, buildFrontRight, buildBackLeft, buildBackRight, lookAtCentroid;
00098     Point frontLeftPt, frontRightPt, backLeftPt, backRightPt, centroid;
00099     int maxRetries, triesLeft;
00100     enum State {none, build, complete, error} myState;
00101   };
00102   
00103   class GotoTarget : public WaypointWalkNode {
00104   public:
00105     GotoTarget() : WaypointWalkNode("GotoTarget"), buildTargetNode(NULL) {}
00106     virtual void DoStart();
00107     
00108     BuildTarget *buildTargetNode;
00109     
00110   protected:
00111     int getNextWaypoint(Point *point, AngSignPi *angle);
00112     void setNextWaypoint();
00113     Point getPointAtAngle(AngSignPi angle);
00114   
00115   
00116   private:
00117     GotoTarget(const GotoTarget&); //!< do not call
00118     GotoTarget& operator=(const GotoTarget&); //!< do not call
00119 };
00120   
00121   class PushTarget : public WaypointWalkNode {
00122   public:
00123     PushTarget() : WaypointWalkNode("PushTarget"), buildTargetNode(NULL) {}
00124     virtual void DoStart();
00125     
00126     BuildTarget *buildTargetNode;
00127   
00128   private:
00129     PushTarget(const PushTarget&); //!< do not call
00130     PushTarget& operator=(const PushTarget&); //!< do not call
00131   };
00132 
00133   class CreepToShape : public WaypointWalkNode {
00134   public:
00135     CreepToShape() : WaypointWalkNode("CreepToShape") {}
00136     virtual void DoStart();
00137   };
00138 
00139   class CreepToShapeStand : public PostureNode {
00140   public:
00141     CreepToShapeStand() : PostureNode("CreepToShapeStand","walk.pos") {}
00142     virtual void DoStop();
00143   };
00144 
00145   class CreepToShapeWait : public StateNode {
00146   public:
00147     CreepToShapeWait() : StateNode("CreepToShapeWait") {}
00148   };
00149 
00150   class CreepToShapeBuildMap : public VisualRoutinesStateNode {
00151   public:
00152     CreepToShapeBuildMap() :  VisualRoutinesStateNode("CreepToShapeBuildMap") {}
00153     virtual void DoStart();
00154     static unsigned int mapreq_id;
00155   };
00156 
00157   class CreepToShapeLocalize : public StateNode {
00158   public:
00159     CreepToShapeLocalize() : StateNode("CreepToShapeLocalize") {}
00160     virtual void DoStart();
00161     virtual void processEvent(const EventBase&);
00162   };
00163 
00164 #ifdef TGT_HAS_IR_DISTANCE
00165 #ifdef TGT_ERS7 
00166   class IRCliff : public StateNode {
00167   public:
00168     IRCliff() : StateNode("IRCliff") {}
00169     virtual void DoStart();
00170     virtual void processEvent(const EventBase&);
00171   };
00172 #endif // TGT_ERS7
00173 
00174   class IRObstacle : public StateNode {
00175   public:
00176     IRObstacle() : StateNode("IRObstacle") {}
00177     virtual void DoStart();
00178     virtual void processEvent(const EventBase&);
00179   };
00180 #endif // TGT_HAS_IR_DISTANCE
00181 
00182   class Localize : public StateNode {
00183   public:
00184     Localize() : StateNode("Localize"), mapbuilder_id(0) {}
00185     virtual void DoStart();
00186     virtual void processEvent(const EventBase&);
00187     void processMap(unsigned int timestamp);
00188     unsigned int mapbuilder_id;
00189   };
00190 
00191   class Dispatch : public VisualRoutinesStateNode {
00192   public:
00193     Dispatch() : 
00194       VisualRoutinesStateNode("Dispatch"), successNode(NULL), failureNode(NULL),
00195       walkNode(NULL), visualsearchNode(NULL), gotoshapeNode(NULL),
00196       gotoTargetNode(NULL), pushTargetNode(NULL),
00197       creeptoshapeNode(NULL),
00198 #ifdef TGT_HAS_IR_DISTANCE
00199 #ifdef TGT_ERS7
00200       ircliffNode(NULL),
00201 #endif
00202       irobstacleNode(NULL),
00203 #endif
00204     localizeNode(NULL)
00205     {}
00206     virtual void setup();
00207     virtual void DoStart();
00208     Success *successNode;
00209     Failure *failureNode;
00210     Walk *walkNode;
00211     VisualSearch *visualsearchNode;
00212     GotoShape *gotoshapeNode;
00213     GotoTarget *gotoTargetNode;
00214     PushTarget *pushTargetNode;
00215     CreepToShape *creeptoshapeNode;
00216 #ifdef TGT_HAS_IR_DISTANCE
00217 #ifdef TGT_ERS7
00218     IRCliff *ircliffNode;
00219 #endif // TGT_ERS7
00220     IRObstacle *irobstacleNode;
00221 #endif // TGT_HAS_IR_DISTANCE
00222     Localize *localizeNode;
00223   private:
00224     Dispatch(const Dispatch&);
00225     Dispatch& operator=(const Dispatch&);
00226   };
00227 
00228   //! Constructor
00229   Pilot();
00230 
00231   virtual void DoStart();
00232   virtual void DoStop();
00233   virtual void setup();
00234   unsigned int executeRequest(const PilotRequest& req);
00235   void abort(); //!< Abort current request and empty the request queue
00236   MotionManager::MC_ID getWaypointWalk_MC_ID() { return waypointwalk_id; }
00237 
00238   static const unsigned int invalid_Pilot_ID = (unsigned int)-1;
00239 
00240 protected:
00241   void executeRequest();
00242   void requestComplete(const bool result=true);
00243   void success() { dispatchNode->successNode->DoStart(); }
00244   void failure() { dispatchNode->failureNode->DoStart(); }
00245 
00246   std::queue<PilotRequest*> requests;
00247   PilotRequest *curReq;
00248   unsigned int idCounter;
00249   MotionManager::MC_ID waypointwalk_id;
00250   MotionManager::MC_ID posture_id;
00251   unsigned int lastDisplayParticleTime;
00252 
00253   Dispatch *dispatchNode;
00254 
00255 private:
00256   Pilot(const Pilot&); //!< do not call
00257   Pilot& operator=(const Pilot&); //!< do not call
00258 
00259 };
00260 
00261 } // namespace
00262 
00263 #endif

DualCoding 4.0
Generated Thu Nov 22 00:52:36 2007 by Doxygen 1.5.4