00001
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&);
00118 GotoTarget& operator=(const GotoTarget&);
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&);
00130 PushTarget& operator=(const PushTarget&);
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
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();
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&);
00257 Pilot& operator=(const Pilot&);
00258
00259 };
00260
00261 }
00262
00263 #endif