00001
00002 #ifndef INCLUDED_Pilot_h_
00003 #define INCLUDED_Pilot_h_
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #ifdef TGT_IS_CHIARA
00015 # define PILOT_USES_SHARED_MOTION_COMMANDS
00016 #endif
00017
00018
00019 # define PILOT_USES_SHARED_MOTION_COMMANDS
00020
00021
00022 #include <queue>
00023 #include <deque>
00024
00025
00026
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
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
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
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);
00133 void cancelVelocity();
00134 void setWorldBounds(float minX, float width, float minY, float height);
00135 void setWorldBounds(const Shape<PolygonData> &bounds);
00136 void randomizeParticles(float widthIncr=1000, float heightIncr=1000);
00137 void computeParticleBounds(float widthIncr=0, float heightIncr=0);
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
00150 static std::vector<ShapeRoot> calculateVisibleLandmarks(const DualCoding::Point ¤tLocation,
00151 AngTwoPi currentOrientation, AngTwoPi maxTurn,
00152 const std::vector<DualCoding::ShapeRoot> &possibleLandmarks);
00153 static bool isLandmarkViewable(const DualCoding::ShapeRoot &selectedLandmark,
00154 const DualCoding::Point ¤tLocation, AngTwoPi currentOrientation, AngTwoPi maxTurn);
00155
00156
00157
00158
00159
00160
00161 static bool defaultLandmarkExitTest();
00162
00163
00164
00165
00166
00167
00168
00169
00170 void setAgent(const Point &loc, AngTwoPi heading, bool quiet=false, bool updateWaypoint=true);
00171
00172
00173
00174
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
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
00191
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;
00202 static unsigned int const collisionEnableTimer = 1;
00203 static unsigned int const overcurrentResetTimer = 2;
00204 };
00205
00206
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;
00212 virtual void doStart() {
00213 VRmixin::isWalkingFlag = false;
00214 VRmixin::pilot->requestComplete(errorType);
00215 }
00216 };
00217
00218
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
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
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
00271 class ExecutePlan : public StateNode {
00272 public:
00273 ExecutePlan(const std::string &nodename="ExecutePlan") : StateNode(nodename), nextDisplacementInstruction() {}
00274
00275
00276 static const float allowableDistanceError;
00277 static const float allowableAngularError;
00278
00279
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();
00299 else
00300 postStateSuccess();
00301 }
00302 };
00303
00304
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
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
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
00335
00336 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00337 float va = (curReq.turnSpeed > 0) ? curReq.turnSpeed : 0.25f;
00338 #else
00339
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
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;
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;
00440 };
00441
00442
00443
00444
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
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
00473 && VRmixin::pilot->curReq->landmarkExitTest() )
00474 postStateSuccess();
00475 else
00476 postStateFailure();
00477 }
00478 };
00479
00480 #ifdef TGT_HAS_HEAD
00481
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
00498
00499
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) {}
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
00519 std::deque<Point> &gazePoints = getAncestor<LocalizationUtility>()->gazePoints;
00520 NEW_SHAPE(gazePoint, PointData, new PointData(VRmixin::localShS, gazePoints.front()));
00521
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
00534 postStateCompletion();
00535 }
00536 void setMC(const MotionManager::MC_ID) {}
00537 };
00538
00539 #else
00540
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;
00675 std::deque<Point> gazePoints;
00676 std::deque<AngTwoPi> gazeHeadings;
00677 };
00678
00679
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
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
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
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
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
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
00856
00857 class GoToShapeMachine : public StateNode {
00858 public:
00859 GoToShapeMachine(const std::string &nodename="GoToShapeMachine") : StateNode(nodename) {}
00860
00861
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() {
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
00993
00994
00995
00996
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
01024
01025
01026
01027
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
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
01057
01058
01059
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
01112
01113
01114
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
01119
01120 AngTwoPi ori = orientation + AngTwoPi(M_PI);
01121
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
01177
01178
01179
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
01184
01185 AngTwoPi ori = orientation + AngTwoPi(M_PI);
01186
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;
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
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
01258
01259
01260
01261
01262
01263
01264
01265
01266
01267
01268
01269
01270
01271
01272
01273
01274
01275
01276
01277
01278
01279
01280
01281
01282
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
01293
01294
01295
01296
01297
01298
01299
01300
01301
01302
01303
01304
01305
01306
01307
01308
01309
01310
01311
01312
01313
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
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
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
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
01400 if (d < backupDist*1.2 && oriDiff < M_PI/180*45) {
01401
01402 std::cout << "Use simple way (straight line) from init to obj" << std::endl;
01403 postStateSuccess();
01404 }
01405 else {
01406
01407 std::cout << "Use complex way (need path planning) from init to obj" << std::endl;
01408 postStateFailure();
01409 }
01410 }
01411 };
01412
01413
01414
01415
01416
01417
01418
01419
01420
01421
01422
01423
01424
01425
01426
01427
01428
01429
01430
01431
01432
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
01447 initToObjPlan.clear();
01448 initToObjPlan.path = plan.path;
01449 initToObjPlan.steps = plan.steps;
01450
01451 Point curPos = VRmixin::theAgent->getCentroid();
01452
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
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
01464 return;
01465 }
01466 Point vec2 = endPos - backupPt;
01467 backupOri = AngTwoPi(std::atan2(vec2.coordY(), vec2.coordX()));
01468
01469 float d = vec.xyNorm();
01470 float oriDiff = std::abs(AngSignPi(ori-backupOri));
01471
01472 if (d < backupDist*1.2 && oriDiff < M_PI/180*45) {
01473
01474 std::cout << "Use simple way (straight line) from obj to dest" << std::endl;
01475 postStateSuccess();
01476 }
01477 else {
01478
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
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
01522
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
01529
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
01541
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
01548
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
01560
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
01582
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
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
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
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
01650
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
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
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
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
01713 PilotRequest &req = *VRmixin::pilot->curReq;
01714 if ( req.objectMatcher == NULL ) {
01715
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
01749
01750 VRmixin::pilot->curReq->objectShape->setPosition(endPos);
01751 }
01752 };
01753
01754 virtual void setup() {
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;
01950 NavigationPlan objToDestPlan;
01951 Point startPos;
01952 Point objPos;
01953 Point endPos;
01954 AngTwoPi startOri;
01955 AngTwoPi acquireOri;
01956 AngTwoPi backupOri;
01957 Point backupPt;
01958 Point acquirePt;
01959 bool useSimpleWayObjToDest;
01960 bool useSimpleWayInitToObj;
01961 bool isObs;
01962 };
01963
01964
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;
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
02065
02066
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:
02092 SetOdometryMachine(const SetOdometryMachine&);
02093 SetOdometryMachine& operator=(const SetOdometryMachine&);
02094 };
02095
02096
02097
02098
02099
02100
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;
02211
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:
02223 Dispatch(const Dispatch&);
02224 Dispatch& operator=(const Dispatch&);
02225 };
02226
02227 virtual void 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&);
02250 Pilot& operator=(const Pilot&);
02251
02252 static PilotVerbosity_t verbosity;
02253
02254 std::queue<PilotRequest*> requests;
02255 PilotRequest *curReq;
02256 unsigned int idCounter;
02257 MotionManager::MC_ID walk_id;
02258 MotionManager::MC_ID waypointwalk_id;
02259 Dispatch *dispatch_;
02260
02261
02262
02263 std::vector<ShapeRoot> defaultLandmarks;
02264 MapBuilderRequest* defaultLandmarkExtractor;
02265 Point initPos;
02266 AngTwoPi initHead;
02267 PilotRequest planReq;
02268 NavigationPlan plan;
02269 AngTwoPi maxTurn;
02270 bool pushingObj;
02271 };
02272
02273 std::ostream& operator<<(std::ostream& os, const NavigationStep &step);
02274 std::ostream& operator<<(std::ostream& os, const NavigationPlan &plan);
02275
02276 }
02277
02278 #endif