Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

KoduInterpreter.h

Go to the documentation of this file.
00001 // INCLUDES
00002 // c++
00003 #include <cctype>
00004 #include <iostream>
00005 #include <queue>
00006 #include <vector>
00007 
00008 // tekkotsu
00009 #include "Behaviors/StateMachine.h"
00010 #include "DualCoding/VRmixin.h"
00011 #include "Motion/WalkMC.h"
00012 #include "Events/EventRouter.h"
00013 
00014 // tekkodu
00015 #include "Kodu/KoduIncludes.h"
00016 #include "Events/Kodu/KoduSayEvent.h"
00017 #include "Events/Kodu/KoduGiveEvent.h"
00018 
00019 // General Functions
00020 #include "Kodu/General/GeneralFncs.h"
00021 #include "Kodu/General/GeneralMacros.h"
00022 #include "Kodu/General/KoduState.h"
00023 
00024 // Kodu Parsing
00025 #include "Kodu/Parsing/Parser.h"
00026 
00027 #include "Kodu/Keepers/ScoreKeeper.h"
00028 #include "Kodu/Keepers/ObjectKeeper.h"
00029 #include "Kodu/KoduPage.h"
00030 #include "Kodu/KoduRule.h"
00031 
00032 
00033 class KoduInterpreter : public VisualRoutinesStateNode {
00034  public:
00035   KoduInterpreter(const std::string &nodename="KoduInterpreter") : VisualRoutinesStateNode(nodename), theWorld(NULL), thisAgent(NULL), discover(NULL), dropActRef(NULL), grabActRef(NULL), giveActRef(NULL), recvActRef(NULL), motionActRef(NULL), pageSwitchActRef(NULL), playActRef(NULL), sayActRef(NULL), scoreActRef(NULL), multiplexorRef(NULL), needToHaltExecution(false), evaluatorRef(NULL), runnerRef(NULL), koduconfig(NULL), elistener(NULL), kodustate(NULL) {}
00036     
00037 
00038     
00039 
00040 
00041 
00042 
00043     
00044     
00045     
00046   ~KoduInterpreter() {
00047     //std::cout << "Destructing Kodu Game...\n";
00048     // null the KoduAgent pointer
00049     thisAgent = NULL;
00050 
00051     // delete the world instance
00052     //std::cout << "Destroying Kodu World.\n";
00053     GeneralFncs::deletePtr(theWorld);
00054     GeneralFncs::deletePtr(discover);
00055     GeneralFncs::deletePtr(koduconfig);
00056     GeneralFncs::deletePtr(elistener);
00057     GeneralFncs::deletePtr(kodustate);
00058 
00059         
00060     //std::cout << "Kodu Game destruction complete!\n";
00061   }
00062 
00063   class KoduEventListener : public StateNode {
00064   private:
00065     Kodu::KoduState* _kodustate;
00066     Kodu::KoduAgent* _thisAgent;
00067 
00068   public:
00069     KoduEventListener(Kodu::KoduState* st, Kodu::KoduAgent* ka) : _kodustate(st), _thisAgent(ka) {}
00070 
00071     KoduEventListener(const KoduEventListener &other); //!< don't call this
00072 
00073     KoduEventListener& operator=(const KoduEventListener &other); //!< don't call this
00074 
00075     void listenTo(int hostAddr) {
00076       erouter->addRemoteListener(this, hostAddr, EventBase::koduEGID);
00077     }
00078 
00079     virtual void doEvent() {
00080       //Ignore self-messages
00081       if (event->getHostID() == -1) return;
00082 
00083       if (dynamic_cast<const KoduSayEvent*>(event) != NULL) {
00084         const KoduSayEvent* sayevent = dynamic_cast<const KoduSayEvent*>(event);
00085         Kodu::KoduState::utterance utt = { sayevent->getHostID(), sayevent->getPhrase() };
00086         _kodustate->addUtterance(utt);
00087       }
00088       if (dynamic_cast<const KoduGiveEvent*>(event) != NULL) {
00089         const KoduGiveEvent* giveevent = dynamic_cast<const KoduGiveEvent*>(event);
00090         if (!_thisAgent->agentIsReceiving && !_thisAgent->agentWantsToGiveObject) {
00091           _thisAgent->setIsReceiving();
00092           _thisAgent->agentReceivingFrom = giveevent->getHostID();
00093                     _thisAgent->gotGiveInformation = false;
00094         }
00095                 else if (_thisAgent->agentWantsToGiveObject) {
00096           if (giveevent->getHostID() == _thisAgent->giveTargetObject->getHostAddr()) {
00097                         _thisAgent->gotGiveReady = true;
00098           }
00099                     else {
00100                         std::cout << "Give event host id (" << giveevent->getHostID() << ") differed from expected"
00101                                   << _thisAgent->giveTargetObject->getHostAddr() << std::endl;
00102                     }
00103                 }
00104                 else if (_thisAgent->agentIsReceiving) {
00105                     if (giveevent->getHostID() == _thisAgent->agentReceivingFrom) {
00106                         _thisAgent->gotGiveInformation = true;
00107                         _thisAgent->giveAngleToTurn = giveevent->getTurn();
00108                         _thisAgent->giveObjType = giveevent->getObjType();
00109                     }
00110                 }
00111       }
00112     }
00113 
00114   };
00115 
00116   static void new_robot_callback(player_identity* ident, void* arg) {
00117     KoduEventListener* el = (KoduEventListener*)arg;
00118     el->listenTo(ident->hostAddr);
00119   }
00120 
00121   class InitializeAgent : public StateNode {
00122    public:
00123     InitializeAgent(const std::string &nodename="InitializeAgent") : StateNode(nodename) {}
00124 
00125     class CreateWorld : public StateNode {
00126      public:
00127       CreateWorld(const std::string &nodename="CreateWorld") : StateNode(nodename) {}
00128       virtual void doStart() {
00129         Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
00130         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
00131         KoduDiscover* &discover = getAncestor<KoduInterpreter>()->discover;
00132         KoduConfig* &koduconfig = getAncestor<KoduInterpreter>()->koduconfig;
00133         KoduEventListener* &elistener = getAncestor<KoduInterpreter>()->elistener;
00134         Kodu::KoduState* &kodustate = getAncestor<KoduInterpreter>()->kodustate;
00135     
00136           //std::cout << "[" << getName() << "]: creating world\n";
00137           // create an instance of the Kodu World
00138           if ((theWorld = new Kodu::KoduWorld()) == NULL) {
00139             //std::cout << "!!! there was an error creating the World! Terminating...\n";
00140             postStateFailure();
00141             return;
00142           }
00143           if ( (kodustate = new Kodu::KoduState()) == NULL ){
00144             //std::cout << "!!! there was an error creating kodu state! Terminating...\n";
00145             postStateFailure();
00146             return;
00147           }
00148           // create a pointer to the agent instance in KoduWorld
00149           thisAgent = &(theWorld->thisAgent);
00150     
00151           if ((koduconfig = new KoduConfig()) == NULL) {
00152             std::cout << "!!! Couldn't create config object\n";
00153             postStateFailure();
00154             return;
00155           }
00156     
00157           if (koduconfig->loadFile("config/KoduConfig.xml") == 0) {
00158             std::cout << "!!! Couldn't load kodu config from ms/config/KoduConfig.xml\n";
00159             postStateFailure();
00160             return;
00161           }
00162     
00163           player_identity ident;
00164           ident.type = koduconfig->kodu_type;
00165     
00166           if ((discover = new KoduDiscover(ident, koduconfig->interface)) == NULL) {
00167             std::cout << "!!! Couldn't create discovery object\n";
00168             postStateFailure();
00169             return;
00170           }
00171     
00172           theAgent->setHostAddr(ident.hostAddr);
00173     
00174           if ((elistener = new KoduEventListener(kodustate, thisAgent)) == NULL) {
00175             std::cout << "!!! Couldn't create KoduEventListener\n";
00176             postStateFailure();
00177             return;
00178           }
00179           elistener->start();
00180           discover->setNewRobotCallback(new_robot_callback, (void*)elistener);
00181           postStateCompletion();
00182       }
00183     };
00184 
00185     class ParseKode : public StateNode {
00186      public:
00187       ParseKode(const std::string &nodename="ParseKode") : StateNode(nodename) {}
00188       virtual void doStart() {
00189           //std::cout << "[" << getName() << "]: parsing kode\n";
00190         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
00191           // parses and creates Kodu program
00192           if (Kodu::Parser::parseAndCreateKoduProgram(thisAgent->pages) == false) {
00193             std::cerr << "!!! Error parsing and creating Kodu program.\n";
00194             postStateFailure();
00195             return;
00196           }
00197           // std::cout << "parsing complete.\n";
00198           postStateSuccess();
00199       }
00200     };
00201 
00202     class LookAround : public MapBuilderNode {
00203      public:
00204       LookAround(const std::string &nodename="LookAround") : MapBuilderNode(nodename,MapBuilderRequest::worldMap) {}
00205       virtual void doStart() {
00206           std::cout << "[" << getName() << "]: looking for objects.\n";
00207           // search for red, blue, and green cylinders
00208           mapreq.addObjectColor(cylinderDataType, "red");
00209           mapreq.addObjectColor(cylinderDataType, "blue");
00210           mapreq.addObjectColor(cylinderDataType, "green");
00211           mapreq.addObjectColor(agentDataType, "red");
00212           mapreq.setAprilTagFamily();
00213                 
00214           // get the gaze points from the agent
00215         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
00216           std::vector<Point> searchPoints(thisAgent->getGazePoints());
00217     
00218           /*
00219             const float kSearchRadius = 1500.0f;
00220             const float kZval = 300.0f;
00221             // now add the approx. search points for the april tags (star constellation)
00222             // direct left
00223             searchPoints.push_back(Point(cos(deg2rad( 90.0f)) * kSearchRadius,
00224             sin(deg2rad( 90.0f)) * kSearchRadius,
00225             kZval, egocentric));
00226     
00227             // forward left
00228             searchPoints.push_back(Point(cos(deg2rad( 35.0f)) * kSearchRadius,
00229             sin(deg2rad( 35.0f)) * kSearchRadius,
00230             kZval, egocentric));
00231     
00232             // forward
00233             searchPoints.push_back(Point(kSearchRadius, 0.0f, kZval, egocentric));
00234     
00235             // forward right
00236             searchPoints.push_back(Point(cos(deg2rad(-35.0f)) * kSearchRadius,
00237             sin(deg2rad(-35.0f)) * kSearchRadius,
00238             kZval, egocentric));
00239     
00240             // direct right
00241             searchPoints.push_back(Point(cos(deg2rad(-90.0f)) * kSearchRadius,
00242             sin(deg2rad(-90.0f)) * kSearchRadius,
00243             kZval, egocentric));
00244           */
00245                 
00246           // create a polygon, and have the robot look at the vertices of the polygon
00247           NEW_SHAPE(gazePoints, PolygonData, new PolygonData(localShS, searchPoints, false));
00248           mapreq.searchArea = gazePoints;
00249           mapreq.removePts = false;
00250       }
00251     };
00252 
00253     class PointHeadFwd : public HeadPointerNode {
00254      public:
00255       PointHeadFwd(const std::string &nodename="PointHeadFwd") : HeadPointerNode(nodename) {}
00256       virtual void doStart() {
00257           // look ahead at point {x, y, z}
00258           getMC()->lookAtPoint(1400.0f, 0, 0);
00259       }
00260     };
00261 
00262     class OrganizeWorld : public StateNode {
00263      public:
00264       OrganizeWorld(const std::string &nodename="OrganizeWorld") : StateNode(nodename) {}
00265       virtual void doStart() {
00266         Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
00267           std::cout << "[" << getName() << "]: organizing the world.\n";
00268           // get all the shapes that are probably false positives, or not considered as a
00269           // world shape, and remove them from the worldShS
00270           std::cout << "deleting some shapes.\n";
00271           NEW_SHAPEROOTVEC(shapesToDelete, subset(worldShS, Kodu::IsNotWorldShape()));
00272           worldShS.deleteShapes(shapesToDelete);
00273           // search for the north star
00274           std::cout << "searching for north star... ";
00275           NEW_SHAPEROOTVEC(constellation, subset(worldShS, Kodu::IsStar()));
00276           // check if the robot observed the constellation
00277           if (!constellation.empty()) {
00278             std::cout << "found the North Star!\n";
00279             // add the constellation (points) to the world state
00280             theWorld->setStarConstellation(constellation);
00281             // generate the world bounds
00282             theWorld->generateWorldBoundsPolygon();
00283             // remove the constellation from the world shape space (prevents the pilot from
00284             // periodically stopping the robot---I want the interpreter to controll that)
00285             worldShS.deleteShapes(constellation);
00286           } else {
00287             std::cout << "could not find North Star (creating an artificial point)\n";
00288             // nothing in the constellation was seen (generate the world bounds)
00289             theWorld->generateWorldBoundsPolygon();
00290           }
00291           // set the world bounds in the pilot
00292           std::cout << "setting world bounds\n";
00293           VRmixin::pilot->setWorldBounds(theWorld->getWorldBoundsPolygon());
00294           // clear the landmark vector in the pilot
00295           std::cout << "clearing default landmarks\n";
00296           VRmixin::pilot->setDefaultLandmarks(std::vector<ShapeRoot>());
00297           postStateCompletion();
00298       }
00299     };
00300 
00301     virtual void setup() {
00302      CreateWorld *newWorld = new CreateWorld("newWorld");
00303      addNode(newWorld);
00304 
00305      ParseKode *parse = new ParseKode("parse");
00306      addNode(parse);
00307 
00308      LookAround *look = new LookAround("look");
00309      addNode(look);
00310 
00311      ParkArm *initArm = new ParkArm("initArm");
00312      addNode(initArm);
00313 
00314      PointHeadFwd *lookFwd = new PointHeadFwd("lookFwd");
00315      addNode(lookFwd);
00316 
00317      OrganizeWorld *orgWorld = new OrganizeWorld("orgWorld");
00318      addNode(orgWorld);
00319 
00320      SpeechNode *speechnode1 = new SpeechNode("speechnode1","error creating world");
00321      addNode(speechnode1);
00322 
00323      PostMachineFailure *postmachinefailure1 = new PostMachineFailure("postmachinefailure1");
00324      addNode(postmachinefailure1);
00325 
00326      PostMachineFailure *postmachinefailure2 = new PostMachineFailure("postmachinefailure2");
00327      addNode(postmachinefailure2);
00328 
00329      PostMachineCompletion *postmachinecompletion1 = new PostMachineCompletion("postmachinecompletion1");
00330      addNode(postmachinecompletion1);
00331 
00332      StateNode *statenode1 = new StateNode("statenode1");
00333      addNode(statenode1);
00334 
00335      startnode = newWorld;
00336 
00337      newWorld->addTransition(new CompletionTrans("completiontrans1",parse));
00338      newWorld->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans1",speechnode1,StateNode::failureSignal));
00339      speechnode1->addTransition(new CompletionTrans("completiontrans2",postmachinefailure1));
00340 
00341      SignalTrans<StateNode::SuccessOrFailure> *signaltrans2 = new SignalTrans<StateNode::SuccessOrFailure>("signaltrans2",look,StateNode::successSignal);
00342      signaltrans2->addDestination(initArm);
00343      parse->addTransition(signaltrans2);
00344 
00345      parse->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans3",postmachinefailure2,StateNode::failureSignal));
00346      look->addTransition(new CompletionTrans("completiontrans3",lookFwd));
00347      lookFwd->addTransition(new CompletionTrans("completiontrans4",orgWorld));
00348      orgWorld->addTransition(new CompletionTrans("completiontrans5",postmachinecompletion1));
00349      initArm->addTransition(new CompletionTrans("completiontrans6",statenode1));
00350     }
00351   };
00352 
00353   class WalkMonitor : public StateNode {
00354    public:
00355     WalkMonitor(const std::string &nodename="WalkMonitor") : StateNode(nodename), lastRecordedState() {}
00356 
00357     virtual void doStart() {
00358       if (VRmixin::isWalkingFlag == true) {
00359         // get the robot's current state
00360         Kodu::PosOrientState currState(VRmixin::theAgent->getCentroid(),
00361                                        VRmixin::theAgent->getOrientation());
00362         // get the difference between points and orientation
00363         Kodu::PosOrientState stateDiff = currState - lastRecordedState;
00364         // motion along the x-axis
00365         float xyNorm = stateDiff.position.xyNorm();
00366         // motion through turning
00367         float arcLen = std::fabs(stateDiff.orientation) * 130.0f;
00368         // increment the total distance travelled
00369     Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
00370         thisAgent->distanceTravelled = thisAgent->distanceTravelled + xyNorm + arcLen;
00371         lastRecordedState = currState;
00372         std::cout << "[" << getName() << "]: accumulated dist. = " 
00373                   << thisAgent->distanceTravelled << "mm\n";
00374       }
00375     }
00376     Kodu::PosOrientState lastRecordedState;   // from $provide
00377   };
00378 
00379   class PerceptualMultiplexor : public StateNode {
00380    public:
00381     PerceptualMultiplexor(const std::string &nodename="PerceptualMultiplexor") : StateNode(nodename), currentTask(NULL), inRecovery(false) {}
00382 
00383     ~PerceptualMultiplexor() {
00384       currentTask = NULL;
00385     }
00386 
00387     enum MultiPlexorTransType_t {
00388       MPT_MAP_BUILDER = 0,
00389       MPT_PILOT
00390     };
00391 
00392     // Returns whether or not the perceptual multiplexor is in recovery mode
00393     bool isInRecovery() const { return inRecovery; }
00394 
00395     class MultiplexorStart : public StateNode {
00396      public:
00397       MultiplexorStart(const std::string &nodename="MultiplexorStart") : StateNode(nodename) {}
00398   
00399         Kodu::PerceptualTaskBase* getNextExecutableTask() {
00400       Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
00401       Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
00402                   
00403           unsigned int tasksToExecute = thisAgent->ptasks.size();
00404           // std::cout << "checking task #'s ";
00405           while (tasksToExecute > 0) {
00406             // std::cout << thisAgent->ptasks.front()->getTaskId() << "; ";
00407             if (thisAgent->ptasks.front()->canExecute(*theWorld)) {
00408               return thisAgent->ptasks.front();
00409             }
00410             tasksToExecute--;
00411             thisAgent->ptasks.push(thisAgent->ptasks.front());
00412             thisAgent->ptasks.pop();
00413           }
00414           std::cout << std::endl;
00415           return NULL;
00416         }
00417   
00418         virtual void doStart() {
00419       Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
00420       Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
00421                   
00422           // std::cout << "=== [" << getName() << "] ===\n";
00423           if (currentTask == NULL) {
00424             if (thisAgent->ptasks.empty()) {
00425               std::cout << "No tasks to execute; Multiplexor exiting.\n";
00426               postParentCompletion();
00427               return;
00428             }
00429                   
00430             std::cout << thisAgent->ptasks.size() << " perceptual tasks to (possibly) execute!\n";
00431             currentTask = getNextExecutableTask();
00432             if ( currentTask == NULL ) {
00433               //std::cout << "no executable tasks left; exiting.\n";
00434               postParentCompletion();
00435               return;
00436             }
00437             // pop the current task off of the queue
00438             thisAgent->ptasks.pop();
00439           }
00440   
00441           std::cout << "Executing task #" << currentTask->getTaskId() << " ";
00442           // check the task type to decide which task runner to use
00443           switch (currentTask->getType()) {
00444           case Kodu::PT_VIS_BUMP_DETECTION:
00445             std::cout << "(Bump Detection).\n";
00446             postStateSignal<MultiPlexorTransType_t>(MPT_MAP_BUILDER);
00447             return;
00448   
00449           case Kodu::PT_GRIPPER_VIS_MONITOR:
00450             std::cout << "(Gripper Monitor).\n";
00451             postStateSignal<MultiPlexorTransType_t>(MPT_MAP_BUILDER);
00452             return;
00453   
00454           case Kodu::PT_VIS_LOCALIZATION:
00455             std::cout << "(Localization).\n";
00456             postStateSignal<MultiPlexorTransType_t>(MPT_PILOT);
00457             return;
00458   
00459           case Kodu::PT_VIS_NAV_ERR_MON:
00460             std::cout << "(VisualNavErrorMon).\n";
00461             postStateSignal<MultiPlexorTransType_t>(MPT_MAP_BUILDER);
00462             return;
00463   
00464           default:
00465             std::cout << "MultiplexorStart: unidentified task found... exiting.\n";
00466             postParentCompletion();
00467             return;
00468           }
00469         }
00470     };
00471 
00472     /**
00473      * The state machine that:
00474      * 1) handles MapBuilder requests, and
00475      * 2) examines the results from the request
00476      **/
00477     class MapBuilderTaskRunner : public StateNode {
00478      public:
00479       MapBuilderTaskRunner(const std::string &nodename="MapBuilderTaskRunner") : StateNode(nodename) {}
00480   
00481       class ExecuteMapBuilderTask : public MapBuilderNode {
00482        public:
00483         ExecuteMapBuilderTask(const std::string &nodename="ExecuteMapBuilderTask") : MapBuilderNode(nodename) {}
00484         virtual void doStart() {
00485               std::cout << "[" << getName() << "]: looking for objects.\n";
00486           Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
00487               // (generate and) get the mapbuilder request
00488               mapreq = currentTask->getMapBuilderRequest();
00489         }
00490       };
00491   
00492       class ExamineMapBuilderResults : public StateNode {
00493        public:
00494         ExamineMapBuilderResults(const std::string &nodename="ExamineMapBuilderResults") : StateNode(nodename) {}
00495         virtual void doStart() {
00496           Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
00497               // examine the results from the mapbuilder request
00498               //cout << " Examine results: currentTask=" << (void*)currentTask << " id=" <<  currentTask->getTaskId() << endl;
00499               currentTask->examineTaskResults();
00500               postParentCompletion();
00501         }
00502       };
00503                                           
00504       virtual void setup() {
00505        ExecuteMapBuilderTask *executeRequest = new ExecuteMapBuilderTask("executeRequest");
00506        addNode(executeRequest);
00507 
00508        ExamineMapBuilderResults *examineResults = new ExamineMapBuilderResults("examineResults");
00509        addNode(examineResults);
00510 
00511        startnode = executeRequest;
00512 
00513        executeRequest->addTransition(new CompletionTrans("completiontrans7",examineResults));
00514       }
00515     };
00516 
00517     // The PilotNode class used for executing Pilot request (such as localization)
00518     class PilotTaskRunner : public StateNode {
00519      public:
00520       PilotTaskRunner(const std::string &nodename="PilotTaskRunner") : StateNode(nodename) {}
00521   
00522       class ExecutePilotTask : public PilotNode {
00523        public:
00524         ExecutePilotTask(const std::string &nodename="ExecutePilotTask") : PilotNode(nodename) {}
00525         virtual void doStart() {
00526           Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
00527               std::cout << "[" << getName() << "]: submitting pilot request for localization\n";
00528               pilotreq = currentTask->getPilotRequest();
00529               if ( pilotreq.requestType > 5 )
00530                 cout << "!!!!!!!!!  Bad pilot request" << endl;
00531         }
00532       };
00533   
00534       class SetTaskSuccess : public StateNode {
00535        public:
00536         SetTaskSuccess(const std::string &nodename="SetTaskSuccess") : StateNode(nodename) {}
00537         virtual void doStart() {
00538           Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
00539               std::cout << "[" << getName() << "]: setting pilot task status ==> SUCCESSFUL\n";
00540               currentTask->setTaskStatus(Kodu::PerceptualTaskBase::TS_SUCCESSFUL);
00541               NEW_SHAPEROOTVEC(stars, subset(worldShS, Kodu::IsStar()));
00542               worldShS.deleteShapes(stars);
00543               postParentCompletion();
00544         }
00545       };
00546   
00547       class SetTaskFailure : public StateNode {
00548        public:
00549         SetTaskFailure(const std::string &nodename="SetTaskFailure") : StateNode(nodename) {}
00550         virtual void doStart() {
00551           Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
00552               std::cout << "[" << getName() << "]: setting pilot task status ==> FAILURE\n";
00553               currentTask->setTaskStatus(Kodu::PerceptualTaskBase::TS_FAILURE);
00554               NEW_SHAPEROOTVEC(stars, subset(worldShS, Kodu::IsStar()));
00555               worldShS.deleteShapes(stars);
00556               postParentCompletion();
00557         }
00558       };
00559   
00560       virtual void setup() {
00561        ExecutePilotTask *executeRequest = new ExecutePilotTask("executeRequest");
00562        addNode(executeRequest);
00563 
00564        SetTaskSuccess *setTaskSuccess = new SetTaskSuccess("setTaskSuccess");
00565        addNode(setTaskSuccess);
00566 
00567        SetTaskFailure *setTaskFailure = new SetTaskFailure("setTaskFailure");
00568        addNode(setTaskFailure);
00569 
00570        startnode = executeRequest;
00571 
00572        executeRequest->addTransition(new PilotTrans("pilottrans1",setTaskSuccess,PilotTypes::noError));
00573        executeRequest->addTransition(new PilotTrans("pilottrans2",setTaskFailure,PilotTypes::cantLocalize));
00574       }
00575     };
00576         
00577     class MultiplexorEnd : public StateNode {
00578      public:
00579       MultiplexorEnd(const std::string &nodename="MultiplexorEnd") : StateNode(nodename) {}
00580       virtual void doStart() {
00581         KoduConditionEvaluator* &evaluatorRef = getAncestor<KoduInterpreter>()->evaluatorRef;
00582         bool &needToHaltExecution = getAncestor<KoduInterpreter>()->needToHaltExecution;
00583         Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
00584         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
00585         Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
00586                 
00587           std::cout << "Task #" << currentTask->getTaskId() << " status: ";
00588           // check if the task is complete
00589           if (currentTask->taskIsComplete(*theWorld)) {
00590             std::cout << "complete! (removing task).\n";
00591             // check if the task failed
00592             if (currentTask->getStatus() == Kodu::PerceptualTaskBase::TS_FAILURE) {
00593         bool &inRecovery = getAncestor<PerceptualMultiplexor>()->inRecovery;
00594               inRecovery = true;
00595               postStateFailure();
00596               return;
00597             }
00598             // delete the task
00599             GeneralFncs::deletePtr(currentTask);
00600           }
00601           // else, the task is not complete so add it to the end of the queue
00602           else {
00603             std::cout << "NOT complete. (adding to end of queue).\n";
00604             thisAgent->ptasks.push(currentTask);
00605           }
00606           currentTask = NULL;
00607           // check if there are more tasks on the queue to execute
00608           if (thisAgent->ptasks.empty()) {
00609             postParentCompletion();
00610           }
00611           else {
00612             // std::cout << "continuing to next task.\n";
00613             postStateCompletion();
00614           }
00615           // this means the evaluator was halted in order to recover, so restart the evaluator
00616           if (needToHaltExecution == true) {
00617             needToHaltExecution = false;
00618             evaluatorRef->start();
00619           }
00620       }
00621     };
00622 
00623     class FailureRecovery : public StateNode {
00624      public:
00625       FailureRecovery(const std::string &nodename="FailureRecovery") : StateNode(nodename) {}
00626               
00627       class PauseInterpretation : public StateNode {
00628        public:
00629         PauseInterpretation(const std::string &nodename="PauseInterpretation") : StateNode(nodename) {}
00630         virtual void doStart() {
00631           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
00632           KoduConditionEvaluator* &evaluatorRef = getAncestor<KoduInterpreter>()->evaluatorRef;
00633           KoduActionRunner* &runnerRef = getAncestor<KoduInterpreter>()->runnerRef;
00634           MotionActionRunner* &motionActRef = getAncestor<KoduInterpreter>()->motionActRef;
00635           DropActionRunner* &dropActRef = getAncestor<KoduInterpreter>()->dropActRef;
00636           GrabActionRunner* &grabActRef = getAncestor<KoduInterpreter>()->grabActRef;
00637           bool &needToHaltExecution = getAncestor<KoduInterpreter>()->needToHaltExecution;
00638                       
00639               std::cout << "[" << getName() << "]: pausing the evaluator.\n";
00640               static unsigned int waitCount = 0;
00641               
00642               // if the halt flag is not set (meaning it is false), set it to true
00643               if (needToHaltExecution == false) {
00644                 std::cout << "Requesting the evaluator to halt.\n";
00645                 needToHaltExecution = true;
00646               }
00647                       
00648               // stop the drop action runner
00649               if (dropActRef->isActive()) {
00650                 std::cout << "Stopping the Grasper (drop action).\n";
00651                 dropActRef->stop();
00652               }
00653       
00654               // stop the grab action runner
00655               if (grabActRef->isActive()) {
00656                 std::cout << "Stopping the Grasper (grab action).\n";
00657                 grabActRef->stop();
00658               }
00659       
00660               // abort/stop any node that may use motion (e.g. Pilot, Grasper)
00661               if (thisAgent->isExecutingMotionAction()) {
00662                 std::cout << "Aborting current pilot request.\n";
00663                 VRmixin::pilot->pilotAbort();
00664                 //thisAgent->agentIsWalking = false;
00665                 thisAgent->motionComplete();
00666                 motionActRef->stop();
00667               }
00668       
00669               // make sure all are inactive before recovery proceeds
00670               if (!evaluatorRef->isActive() && !thisAgent->isExecutingMotionAction()
00671                   && !runnerRef->isActive() && !grabActRef->isActive())
00672                 {
00673                   if ((++waitCount) == 2) {
00674                     waitCount = 0;
00675                     postStateCompletion();
00676                   }
00677                 }
00678               // else, wait for the timeout transition
00679               else {
00680                 PRINT_ATTRS("Evaluator is active?", evaluatorRef->isActive());
00681                 PRINT_ATTRS("Runner is active?", runnerRef->isActive());
00682                 PRINT_ATTRS("Agent is walking?", thisAgent->isExecutingMotionAction());
00683                 PRINT_ATTRS("Grab Action is active?", grabActRef->isActive());
00684                 waitCount = 0;
00685               }
00686         }
00687       };
00688   
00689       class StartRecovery : public StateNode {
00690        public:
00691         StartRecovery(const std::string &nodename="StartRecovery") : StateNode(nodename) {}
00692         virtual void doStart() {
00693           Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
00694               std::cout << "[" << getName() << "]\n";
00695               std::cout << "Perceptual Recovery starting...\n";
00696               // the signal type decides which recovery is started
00697               postStateSignal<Kodu::PerceptualTaskType_t>(currentTask->getType());
00698         }
00699       };
00700   
00701       class ObjectManipRecovery : public StateNode {
00702        public:
00703         ObjectManipRecovery(const std::string &nodename="ObjectManipRecovery") : StateNode(nodename), gTask(NULL), grasperTarget(), objectTagId(-1) {}
00704             
00705             ~ObjectManipRecovery() {
00706               gTask = NULL;
00707             }
00708     
00709         class InitiateManipRecovery : public StateNode {
00710          public:
00711           InitiateManipRecovery(const std::string &nodename="InitiateManipRecovery") : StateNode(nodename) {}
00712       
00713           class GetObjectInfo : public StateNode {
00714            public:
00715             GetObjectInfo(const std::string &nodename="GetObjectInfo") : StateNode(nodename) {}
00716             virtual void doStart() {
00717               Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
00718               Kodu::VisualGripperMonitorTask* &gTask = getAncestor<ObjectManipRecovery>()->gTask;
00719               int &objectTagId = getAncestor<ObjectManipRecovery>()->objectTagId;
00720                       
00721                       std::cout << "[" << getName() << "]: getting the gripper monitor task\n";
00722                       // cast the current task, and point to it
00723                       gTask = static_cast<Kodu::VisualGripperMonitorTask*>(currentTask);
00724                       objectTagId = gTask->getTagId();
00725                       postStateCompletion();
00726             }
00727           };
00728       
00729           class OpenGripper : public ArmNode {
00730            public:
00731             OpenGripper(const std::string &nodename="OpenGripper") : ArmNode(nodename) {}
00732             virtual void doStart() {
00733                       std::cout << "[" << getName() << "]: opening the gripper\n";
00734                       // opens the gripper in case it detected a false positive (the object is
00735                       // still in the gripper, but the robot detected a failure)
00736                       getMC()->openGripper(0.8f);
00737             }
00738           };
00739       
00740           virtual void setup() {
00741            GetObjectInfo *getInfo = new GetObjectInfo("getInfo");
00742            addNode(getInfo);
00743 
00744            OpenGripper *openGrip = new OpenGripper("openGrip");
00745            addNode(openGrip);
00746 
00747            PostMachineCompletion *postmachinecompletion2 = new PostMachineCompletion("postmachinecompletion2");
00748            addNode(postmachinecompletion2);
00749 
00750            startnode = getInfo;
00751 
00752            getInfo->addTransition(new CompletionTrans("completiontrans8",openGrip));
00753            openGrip->addTransition(new CompletionTrans("completiontrans9",postmachinecompletion2));
00754           }
00755         };
00756     
00757         class PrepareForItemRecovery : public StateNode {
00758          public:
00759           PrepareForItemRecovery(const std::string &nodename="PrepareForItemRecovery") : StateNode(nodename), reverseDist(0.0f) {}
00760       
00761           class Reverse : public PilotNode {
00762            public:
00763             Reverse(const std::string &nodename="Reverse") : PilotNode(nodename,PilotTypes::walk) {}
00764             virtual void doStart() {
00765                       std::cout << "[" << getName() << "]: ";
00766                       //$reference KoduInterpreter::thisAgent;
00767               Kodu::VisualGripperMonitorTask* &gTask = getAncestor<ObjectManipRecovery>()->gTask;
00768               float &reverseDist = getAncestor<PrepareForItemRecovery>()->reverseDist;
00769                       // get the current and last positions
00770                       Point lastPos = gTask->getLastSuccessfulState().position;
00771                       Point currPos = VRmixin::theAgent->getCentroid();
00772                       // get the gripper object's radius
00773                       //float gripperObjectRadius = Kodu::objectRadius(thisAgent->gripperObject);
00774                       // get the distance the robot travelled between position states
00775                       float positionMag = (currPos - lastPos).xyNorm();
00776                       // calculate how far the robot should reverse
00777                       //reverseDist = (gripperObjectRadius * 1.25f) + positionMag;
00778                       reverseDist = 75.0f + positionMag;
00779                       // set the distance the robot should reverse
00780                       pilotreq.dx = -1.0f * reverseDist;
00781                       std::cout << "reversing by " << reverseDist << "mm ("
00782                                 << 75.0f << " + " << positionMag << ")\n";
00783                       //<< gripperObjectRadius << " + " << positionMag << ")\n";
00784             }
00785           };
00786       
00787           class LocateLostObject : public MapBuilderNode {
00788            public:
00789             LocateLostObject(const std::string &nodename="LocateLostObject") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
00790             virtual void doStart() {
00791               Kodu::VisualGripperMonitorTask* &gTask = getAncestor<ObjectManipRecovery>()->gTask;
00792                       std::cout << "[" << getName() << "]: "
00793                                 << "creating mapbuilder request to find object\n";
00794                       // get the robot's state when it last saw the april tag in the correct location
00795                       Kodu::PosOrientState lastState = gTask->getLastSuccessfulState();
00796                       float lastOrient = lastState.orientation;
00797                                   
00798                       // get the robot's current (position, orientation) state
00799                       Kodu::PosOrientState currState(VRmixin::theAgent->getCentroid(),
00800                                                      static_cast<float>(VRmixin::theAgent->getOrientation()));
00801                       float currOrient = currState.orientation;
00802                                   
00803                       // get the difference between the last and current orientations
00804                       float orientDiff = AngTwoPi(lastOrient - currOrient);
00805                       std::cout << "currOrient = " << currOrient << " rads (= " 
00806                                 << rad2deg(currOrient) << " degs); lastOrient = "
00807                                 << lastOrient << " rads (= " << rad2deg(lastOrient)
00808                                 << " degs); orientDiff = " << orientDiff << " rads (= "
00809                                 << rad2deg(orientDiff) << " degs)\n";
00810           
00811                       const Shape<CylinderData>& kLostCyl =
00812                         ShapeRootTypeConst(gTask->getGripperObject(), CylinderData);
00813                                   
00814                       // create constants that will be used to tell the robot where to search
00815                       // the over approx. max reach the gripper has (measured from the robot's center)
00816                       float const kMaxGripperReach = 400.0f;
00817                       // the cylinder's height
00818                       float const kCylHeight = kLostCyl->getHeight();
00819                       // the cylinder's radius
00820                       float const kCylRadius = kLostCyl->getRadius();
00821                       // the distance the robot reversed
00822               float &reverseDist = getAncestor<PrepareForItemRecovery>()->reverseDist;
00823                       // the max distance the object should be away from the robot's centroid
00824                       float dropRadius = kMaxGripperReach + kCylRadius + reverseDist;
00825                       // the max distance an object can be for the robot to consider
00826                       // it as the lost object
00827                       float maxSearchRadius = dropRadius + (kCylRadius * 2.0f);
00828                                   
00829                       // find out what the robot was doing when the object fell out of the gripper
00830                       float const kInitAngle = gTask->getTagCentroid().atanYX();
00831                       float thetaIncrement = 0.0f;
00832                       float const kIncrementVal = deg2rad(15.0f);
00833                       if (std::fabs(lastOrient - currOrient) > M_PI) {
00834                         std::cout << "Robot crossed over the 360 deg mark (abs(orientDiff) = "
00835                                   << std::fabs(lastOrient - currOrient) << ")\n";
00836                         if (currOrient < lastOrient) {
00837                           std::cout << "currOrient (" << currOrient << ") < lastOrient ("
00838                                     << lastOrient << ")\n";
00839                           currOrient = currOrient + (2 * M_PI);
00840                         } else {
00841                           std::cout << "currOrient (" << currOrient << ") > lastOrient ("
00842                                     << lastOrient << ")\n";
00843                           lastOrient = lastOrient + (2 * M_PI);
00844                         }
00845                         std::cout << "new values: currOrient = " << currOrient
00846                                   << "; lastOrient = " << lastOrient << std::endl;
00847                       }
00848                                   
00849                       float absOrientDiff = std::fabs(lastOrient - currOrient);
00850                       if (currOrient > lastOrient) {
00851                         thetaIncrement = -1.0f * kIncrementVal;
00852                         std::cout << "Robot was turning left; ";
00853                       } else if (currOrient < lastOrient) {
00854                         thetaIncrement = kIncrementVal;
00855                         std::cout << "Robot was turning right; ";
00856                       } else {
00857                         thetaIncrement = 0.0f;
00858                         std::cout << "The object should be in front of the gripper; ";
00859                       }
00860                       std::cout << "thetaIncrement = " << thetaIncrement << " rads (= "
00861                                 << rad2deg(thetaIncrement) << " degs)\n";
00862           
00863                       // start from the current orientation and progressively look to the last state
00864                       std::vector<Point> pts;
00865                       int const kNumbOfPoints = (int)(std::ceil(absOrientDiff / kIncrementVal));
00866                       pts.reserve(kNumbOfPoints + 1);
00867                       std::cout << "the robot will have to look " << (kNumbOfPoints + 1)
00868                                 << " time(s).\n";
00869                       float currAngle = kInitAngle;
00870                       std::cout << "currAngle = " << currAngle << " rads (= " << (rad2deg(currAngle))
00871                                 << " degs) ";
00872                                   
00873                       // approx. position of the object if it was still in the gripper
00874                       pts.push_back(Point(cos(currAngle) * dropRadius, sin(currAngle) * dropRadius,
00875                                           kCylHeight, egocentric));
00876                       std::cout << "begin pt = " << pts[0] << std::endl;
00877           
00878                       // generate the rest of the look points
00879                       for (int i = 0; i < kNumbOfPoints; i++) {
00880                         currAngle += thetaIncrement;
00881                         std::cout << "currAngle = " << currAngle << " rads (= "
00882                                   << (rad2deg(currAngle)) << " degs) ";
00883                         pts.push_back(Point(cos(currAngle) * dropRadius,
00884                                             sin(currAngle) * dropRadius,
00885                                             kCylHeight, egocentric));
00886                         std::cout << "added pt #" << pts.size() << " = " << pts[pts.size() - 1]
00887                                   << std::endl;
00888                       }
00889           
00890                       // create the gaze polygon (the points the robot should look at)
00891                       NEW_SHAPE(objSearchPoints, PolygonData, new PolygonData(localShS, pts, false));
00892                       objSearchPoints->setViewable(true);
00893                       objSearchPoints->setObstacle(false);
00894                                   
00895                       // create the mapbuilder request
00896                       mapreq.searchArea = objSearchPoints;
00897                       mapreq.clearLocal = true;
00898                       mapreq.maxDist = maxSearchRadius;
00899                       //mapreq.pursueShapes = true;
00900                       mapreq.removePts = false;
00901                       mapreq.setAprilTagFamily();
00902                       mapreq.addObjectColor(kLostCyl->getType(), kLostCyl->getColor());
00903             }
00904           };
00905       
00906           class SetGrasperTarget : public StateNode {
00907            public:
00908             SetGrasperTarget(const std::string &nodename="SetGrasperTarget") : StateNode(nodename) {}
00909             virtual void doStart() {
00910               Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
00911               ShapeRoot &grasperTarget = getAncestor<ObjectManipRecovery>()->grasperTarget;
00912               int &objectTagId = getAncestor<ObjectManipRecovery>()->objectTagId;
00913           
00914                       std::cout << "[" << getName() << "]: " << "setting grasper target, id=" << objectTagId << "!\n";
00915           
00916                       ShapeRoot tag = find_if(localShS, Kodu::HasAprilTagID(objectTagId));
00917                       NEW_SHAPEROOTVEC(cyls, subset(localShS, Kodu::IsShapeOfType(cylinderDataType)));
00918                                   
00919                       if (tag.isValid()) {
00920                         if (!cyls.empty()) {
00921                           std::cout << "tag is valid!\n";
00922                           grasperTarget = Kodu::getClosestObjectToPoint(cyls, tag->getCentroid());
00923                         }
00924                         // tag was seen, but not the cylinder
00925                         else {
00926                           const Shape<CylinderData>& kGripCyl
00927                             = ShapeRootTypeConst(thisAgent->gripperObject, CylinderData);
00928                           const Point& kTagPt = tag->getCentroid();
00929                           float cylHeight = kGripCyl->getHeight();
00930                           float cylRadius = kGripCyl->getRadius();
00931                           Point lclCylPt = Point(kTagPt.coordX(), kTagPt.coordY(),
00932                                                  (kTagPt.coordZ() / 2.0f), egocentric);
00933                                           
00934                           NEW_SHAPE(duplicate, CylinderData,
00935                                     new CylinderData(localShS, lclCylPt, cylHeight, cylRadius));
00936                           grasperTarget = duplicate;
00937                         }
00938                       }
00939           
00940                       // the tag was not visible
00941                       // if (!cyls.empty()) {
00942                       //     std::cout << "tag is NOT valid. getting the closest match.\n";
00943                       //     grasperTarget = find_if(localShS, Kodu::IsShapeOfType(cylinderDataType));
00944                       // }
00945           
00946                       if (grasperTarget.isValid()) {
00947                         std::cout << "grasper target was valid!\n";
00948                         grasperTarget = VRmixin::mapBuilder->importLocalShapeToWorld(grasperTarget);
00949                         postStateSuccess();
00950                       } else {
00951                         std::cout << "grasper target was not valid!!!\n";
00952                         postStateFailure();
00953                       }
00954             }
00955           };
00956       
00957           class FaceTarget : public PilotNode {
00958            public:
00959             FaceTarget(const std::string &nodename="FaceTarget") : PilotNode(nodename,PilotTypes::walk) {}
00960             virtual void doStart() {
00961               ShapeRoot &grasperTarget = getAncestor<ObjectManipRecovery>()->grasperTarget;
00962                       std::cout << "[" << getName() << "]: turning by ";
00963                       float bearingToTarget = Kodu::bearingFromAgentToObject(grasperTarget);
00964                       std::cout << bearingToTarget << " rads (= " << rad2deg(bearingToTarget)
00965                                 << " degs)\n";
00966                       pilotreq.da = bearingToTarget;
00967             }
00968           };
00969       
00970           virtual void setup() {
00971            Reverse *rvrs = new Reverse("rvrs");
00972            addNode(rvrs);
00973 
00974            LocateLostObject *find = new LocateLostObject("find");
00975            addNode(find);
00976 
00977            SetGrasperTarget *setTarget = new SetGrasperTarget("setTarget");
00978            addNode(setTarget);
00979 
00980            FaceTarget *faceTarget = new FaceTarget("faceTarget");
00981            addNode(faceTarget);
00982 
00983            PostMachineSuccess *postmachinesuccess1 = new PostMachineSuccess("postmachinesuccess1");
00984            addNode(postmachinesuccess1);
00985 
00986            PostMachineFailure *postmachinefailure3 = new PostMachineFailure("postmachinefailure3");
00987            addNode(postmachinefailure3);
00988 
00989            startnode = rvrs;
00990 
00991            rvrs->addTransition(new CompletionTrans("completiontrans10",find));
00992            find->addTransition(new CompletionTrans("completiontrans11",setTarget));
00993            setTarget->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans4",faceTarget,StateNode::successSignal));
00994            faceTarget->addTransition(new CompletionTrans("completiontrans12",postmachinesuccess1));
00995            setTarget->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans5",postmachinefailure3,StateNode::failureSignal));
00996           }
00997           float reverseDist;   // from $provide
00998         };
00999     
01000             // ASSUMPTION:
01001             // --- there is only one tag in view (need to use ID to select correct object)
01002         class RetrieveLostObject : public GrasperNode {
01003          public:
01004           RetrieveLostObject(const std::string &nodename="RetrieveLostObject") : GrasperNode(nodename,GrasperRequest::grasp) {}
01005           virtual void doStart() {
01006             ShapeRoot &grasperTarget = getAncestor<ObjectManipRecovery>()->grasperTarget;
01007                   std::cout << "[" << getName() << "]: going to retrieve the lost object...\n";
01008                   graspreq.object = grasperTarget;
01009           }
01010         };
01011     
01012         class VerifyObjectWasGrabbed : public StateNode {
01013          public:
01014           VerifyObjectWasGrabbed(const std::string &nodename="VerifyObjectWasGrabbed") : StateNode(nodename) {}
01015       
01016           class LookAtTheGripper : public MapBuilderNode {
01017            public:
01018             LookAtTheGripper(const std::string &nodename="LookAtTheGripper") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
01019             virtual void doStart() {
01020                       std::cout << "[" << getName() << "]: looking at tag\n";
01021                       mapreq.clearLocal = true;
01022                       // search for an april tag
01023                       mapreq.setAprilTagFamily();
01024                       // look at the gripper
01025                       fmat::Column<3> gripperLoc;
01026           #if defined(TGT_IS_CALLIOPE2)
01027                       gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
01028           #endif
01029                       NEW_SHAPE(gazePoint, PointData, new PointData(localShS,
01030                                                                     Point(gripperLoc[0], gripperLoc[1], gripperLoc[2], egocentric)));
01031                       mapreq.searchArea = gazePoint;
01032             }
01033           };
01034       
01035           class VerifyObjectInGripper : public StateNode {
01036            public:
01037             VerifyObjectInGripper(const std::string &nodename="VerifyObjectInGripper") : StateNode(nodename) {}
01038             virtual void doStart() {
01039               Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
01040               Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
01041                       std::cout << "[" << getName() << "]: verifying object was grabbed.\n";
01042                       
01043                       // check if a particular tag was seen
01044                       NEW_SHAPE(tag, AprilTagData,
01045                                 find_if<AprilTagData>(localShS,
01046                                                       Kodu::HasAprilTagID(theWorld->getTagIdForShape(thisAgent->gripperObject.getId()))));
01047                       
01048                       // if the reference is not valid, post a failure
01049                       if (!tag.isValid()) {
01050                         std::cout << "THE TAG WAS NOT SEEN (FAILURE)!!!\n";
01051                         postStateFailure();
01052                         return;
01053                       }
01054           
01055                       // ********** temp fix
01056                       // 
01057                       // get the gripper location
01058                       fmat::Column<3> gripperLoc;
01059           #if defined(TGT_IS_CALLIOPE2)
01060                       gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
01061           #endif
01062                       Point gripperPoint(gripperLoc[0], gripperLoc[1], gripperLoc[2], egocentric);
01063                       
01064                       // check if the object is in the gripper
01065                       if (tag->getCentroid().xyDistanceFrom(gripperPoint)
01066                           < (Kodu::objectRadius(thisAgent->gripperObject) * 1.25f)) {
01067                         std::cout << "successfully grabbed object\n";
01068                         postParentSuccess();
01069                       } else {
01070                         std::cout << "failed to grab object... dist = "
01071                                   << tag->getCentroid().xyDistanceFrom(gripperPoint)
01072                                   << "mm; reattempting.\n";
01073                         postParentFailure();
01074                       }
01075                       // ***********
01076             }
01077           };
01078       
01079           virtual void setup() {
01080            LookAtTheGripper *lookAtGripper = new LookAtTheGripper("lookAtGripper");
01081            addNode(lookAtGripper);
01082 
01083            VerifyObjectInGripper *verifyObjGrabbed = new VerifyObjectInGripper("verifyObjGrabbed");
01084            addNode(verifyObjGrabbed);
01085 
01086            startnode = lookAtGripper;
01087 
01088            lookAtGripper->addTransition(new CompletionTrans("completiontrans13",verifyObjGrabbed));
01089           }
01090         };
01091     
01092         class PrepareForAnotherGrasp : public StateNode {
01093          public:
01094           PrepareForAnotherGrasp(const std::string &nodename="PrepareForAnotherGrasp") : StateNode(nodename), objFutureEgoPos() {}
01095       
01096           class NeedToLookAround : public StateNode {
01097            public:
01098             NeedToLookAround(const std::string &nodename="NeedToLookAround") : StateNode(nodename) {}
01099             virtual void doStart() {
01100               int &objectTagId = getAncestor<ObjectManipRecovery>()->objectTagId;
01101                                   
01102                       std::cout << "[" << getName() << "]: checking if robot needs to look around\n";
01103                       // get a particular april tag
01104                       ShapeRoot tag = find_if(localShS, Kodu::HasAprilTagID(objectTagId));
01105                       postStateSignal<bool>(!tag.isValid());
01106             }
01107           };
01108       
01109           class LookForTag : public MapBuilderNode {
01110            public:
01111             LookForTag(const std::string &nodename="LookForTag") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
01112             virtual void doStart() {
01113                       std::cout << "[" << getName() << "]: attempting to locate object (again)\n";
01114                       // generate points to look at (near the robot's body)
01115                       // these points could also be generated by looking at the gripper's y-pos
01116                       std::vector<Point> pts;
01117                       pts.push_back(Point(  300,    0,    0, egocentric));
01118                       pts.push_back(Point(  500,    0,    0, egocentric));
01119                       pts.push_back(Point(  500, -200,    0, egocentric));
01120                       pts.push_back(Point(  300, -200,    0, egocentric));
01121                       NEW_SHAPE(gazePoints, PolygonData, new PolygonData(localShS, pts, false));
01122                                   
01123                       // construct the mapbuilder request
01124                       mapreq.searchArea = gazePoints;
01125                       mapreq.setAprilTagFamily();
01126                       mapreq.clearLocal = true;
01127             }
01128           };
01129       
01130           class RepositionBody : public PilotNode {
01131            public:
01132             RepositionBody(const std::string &nodename="RepositionBody") : PilotNode(nodename,PilotTypes::walk) {}
01133             virtual void doStart() {
01134               Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
01135               int &objectTagId = getAncestor<ObjectManipRecovery>()->objectTagId;
01136               Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
01137                                   
01138                       std::cout << "[" << getName() << "]: walking distance = ";
01139                       // get a particular april tag
01140                       ShapeRoot tag = find_if(localShS, Kodu::HasAprilTagID(objectTagId));
01141           
01142                       // get the gripper location
01143                       fmat::Column<3> gripperLoc;
01144           #if defined(TGT_IS_CALLIOPE2)
01145                       gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
01146           #endif
01147                       // where I would really want the objetc to be is the radial reach of the robot
01148                       // plus 50 (or 60) mm
01149                       float objDesiredXPos = gripperLoc[0] + 150.0f;
01150                                   
01151                       const Point& kTagPt = tag->getCentroid();
01152                       float objRadius = Kodu::objectRadius(thisAgent->gripperObject);
01153                                   
01154                       pilotreq.dx = kTagPt.coordX() - objRadius - objDesiredXPos;
01155                       std::cout << pilotreq.dx << "mm\n";
01156                                   
01157                       // reposition the gripper target object
01158                       objFutureEgoPos = Point(objDesiredXPos, kTagPt.coordY(),
01159                                               (kTagPt.coordZ() / 2.0f), egocentric);
01160                       pilotreq.collisionAction = collisionIgnore;
01161             }
01162           };
01163       
01164           class FindObjectAgain : public MapBuilderNode {
01165            public:
01166             FindObjectAgain(const std::string &nodename="FindObjectAgain") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
01167             virtual void doStart() {
01168               Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
01169               Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
01170           
01171                       std::cout << "[" << getName() << "]: looking for gripper object (again)\n";
01172                       // search for the object again using the point calculated previously
01173                       NEW_SHAPE(gazePoint, PointData, new PointData(localShS, objFutureEgoPos));
01174                       mapreq.searchArea = gazePoint;
01175                       const ShapeRoot& targetShape = thisAgent->gripperObject;
01176                       mapreq.addAttributes(targetShape);
01177                       mapreq.maxDist = objFutureEgoPos.xyNorm()
01178                         + (Kodu::objectRadius(targetShape) * 2.5f);
01179                       mapreq.pursueShapes = true;
01180             }
01181           };
01182       
01183           class RepositionGripperObject : public StateNode {
01184            public:
01185             RepositionGripperObject(const std::string &nodename="RepositionGripperObject") : StateNode(nodename) {}
01186             virtual void doStart() {
01187               Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
01188               ShapeRoot &grasperTarget = getAncestor<ObjectManipRecovery>()->grasperTarget;
01189               Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
01190                                   
01191                       std::cout << "[" << getName() << "]: attempting to reposition gripper object.\n";
01192                       NEW_SHAPEROOTVEC(objs, subset(localShS,
01193                                                     Kodu::IsShapeOfType(thisAgent->gripperObject->getType())));
01194                       ShapeRoot targetShape = Kodu::getClosestObjectToPoint(objs, objFutureEgoPos);
01195                       // check if the target shape is valid
01196                       if (targetShape.isValid()) {
01197                         std::cout << "target shape is valid. setting its allocentric position.\n";
01198                         if (grasperTarget.isValid()) {
01199                           Point objAllocentricPt = targetShape->getCentroid();
01200                           objAllocentricPt.applyTransform(VRmixin::mapBuilder->localToWorldMatrix,
01201                                                           allocentric);
01202                           std::cout << "Repositioning the object to from "
01203                                     << grasperTarget->getCentroid() << " to "
01204                                     << objAllocentricPt << std::endl;
01205                           grasperTarget->setPosition(objAllocentricPt);
01206                         } else {
01207                           std::cout << "importing shape " << targetShape << " to the worldShS.\n";
01208                           grasperTarget =VRmixin::mapBuilder->importLocalShapeToWorld(targetShape);
01209                           std::cout << "target @ " << grasperTarget->getCentroid() << std::endl;
01210                         }
01211                         postStateSuccess();
01212                       }
01213                       // should only fail if the robot did not see any cylinders
01214                       else {
01215                         std::cout << "target shape is NOT VALID!!!! Don't know what to do!\n";
01216                         grasperTarget = ShapeRoot();
01217                         postStateFailure();
01218                       }
01219             }
01220           };
01221       
01222           virtual void setup() {
01223            NeedToLookAround *needToLook = new NeedToLookAround("needToLook");
01224            addNode(needToLook);
01225 
01226            LookForTag *lookForTag = new LookForTag("lookForTag");
01227            addNode(lookForTag);
01228 
01229            RepositionBody *reposBody = new RepositionBody("reposBody");
01230            addNode(reposBody);
01231 
01232            FindObjectAgain *findObjAgain = new FindObjectAgain("findObjAgain");
01233            addNode(findObjAgain);
01234 
01235            RepositionGripperObject *reposGripObj = new RepositionGripperObject("reposGripObj");
01236            addNode(reposGripObj);
01237 
01238            PostMachineSuccess *postmachinesuccess2 = new PostMachineSuccess("postmachinesuccess2");
01239            addNode(postmachinesuccess2);
01240 
01241            SpeechNode *speechnode2 = new SpeechNode("speechnode2","error shape was not valid");
01242            addNode(speechnode2);
01243 
01244            PostMachineFailure *postmachinefailure4 = new PostMachineFailure("postmachinefailure4");
01245            addNode(postmachinefailure4);
01246 
01247            startnode = needToLook;
01248 
01249            needToLook->addTransition(new SignalTrans<bool>("signaltrans6",lookForTag,true));
01250            lookForTag->addTransition(new CompletionTrans("completiontrans14",reposBody));
01251            needToLook->addTransition(new SignalTrans<bool>("signaltrans7",reposBody,false));
01252            reposBody->addTransition(new CompletionTrans("completiontrans15",findObjAgain));
01253            findObjAgain->addTransition(new CompletionTrans("completiontrans16",reposGripObj));
01254            reposGripObj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans8",postmachinesuccess2,StateNode::successSignal));
01255            reposGripObj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans9",speechnode2,StateNode::failureSignal));
01256            speechnode2->addTransition(new CompletionTrans("completiontrans17",postmachinefailure4));
01257           }
01258           Point objFutureEgoPos;   // from $provide
01259         };
01260     
01261             // ASSUMPTION: only one tag is in view
01262         class GetTagLocation : public StateNode {
01263          public:
01264           GetTagLocation(const std::string &nodename="GetTagLocation") : StateNode(nodename) {}
01265           virtual void doStart() {
01266             Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
01267             Kodu::VisualGripperMonitorTask* &gTask = getAncestor<ObjectManipRecovery>()->gTask;
01268                   std::cout << "[" << getName() << "]: getting april tag's location...\n";
01269         
01270                   int tagId = theWorld->getTagIdForShape(theWorld->thisAgent.gripperObject.getId());
01271                   ShapeRoot objtag = find_if(localShS, Kodu::HasAprilTagID(tagId));
01272         
01273                   if (objtag.isValid()) {
01274                     std::cout << "repositioning tag's location to " << objtag->getCentroid()
01275                               << std::endl;
01276                     gTask->relocateTagCentroid(objtag->getCentroid());
01277                     std::cout << "changing task status to IN PROGRESS\n";
01278                     gTask->setTaskStatus(Kodu::PerceptualTaskBase::TS_IN_PROGRESS);
01279                     postStateSuccess();
01280                   } else {
01281                     std::cout << "object tag is NOT VALID!!!\n";
01282                     postStateFailure();
01283                   }
01284                   // book keeping
01285                   gTask = NULL;
01286                   // ********** possible temp fix
01287             ShapeRoot &grasperTarget = getAncestor<ObjectManipRecovery>()->grasperTarget;
01288                   std::cout << "grasper target valid? ";
01289                   if (grasperTarget.isValid()) {
01290                     std::cout << "yes\n";
01291                     std::cout << "grasper target's matches the gripper target?";
01292             Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
01293                     if (grasperTarget.getId() == thisAgent->gripperObject.getId()) {
01294                       std::cout << "yes, not deleting... making it not an obstacle.\n";
01295                       grasperTarget->setObstacle(false);
01296                     } else {
01297                       std::cout << "no, deleting the object.\n";
01298                       worldShS.deleteShape(grasperTarget);
01299                     }
01300                   } else {
01301                     std::cout << "no, THIS SHOULD NOT HAPPEN!!!\n";
01302                   }
01303                   // ***********
01304           }
01305         };
01306     
01307         virtual void setup() {
01308          InitiateManipRecovery *initMRec = new InitiateManipRecovery("initMRec");
01309          addNode(initMRec);
01310 
01311          PrepareForItemRecovery *prep4ItemRcvry = new PrepareForItemRecovery("prep4ItemRcvry");
01312          addNode(prep4ItemRcvry);
01313 
01314          RetrieveLostObject *retrvObj = new RetrieveLostObject("retrvObj");
01315          addNode(retrvObj);
01316 
01317          VerifyObjectWasGrabbed *verifyRetrv = new VerifyObjectWasGrabbed("verifyRetrv");
01318          addNode(verifyRetrv);
01319 
01320          PrepareForAnotherGrasp *prep4Grasp = new PrepareForAnotherGrasp("prep4Grasp");
01321          addNode(prep4Grasp);
01322 
01323          GetTagLocation *getLoc = new GetTagLocation("getLoc");
01324          addNode(getLoc);
01325 
01326          SpeechNode *speechnode3 = new SpeechNode("speechnode3","could not find the object");
01327          addNode(speechnode3);
01328 
01329          PostMachineFailure *postmachinefailure5 = new PostMachineFailure("postmachinefailure5");
01330          addNode(postmachinefailure5);
01331 
01332          PostMachineSuccess *postmachinesuccess3 = new PostMachineSuccess("postmachinesuccess3");
01333          addNode(postmachinesuccess3);
01334 
01335          startnode = initMRec;
01336 
01337          initMRec->addTransition(new CompletionTrans("completiontrans18",prep4ItemRcvry));
01338          prep4ItemRcvry->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans10",retrvObj,StateNode::successSignal));
01339          prep4ItemRcvry->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans11",prep4Grasp,StateNode::failureSignal));
01340          prep4Grasp->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans12",retrvObj,StateNode::successSignal));
01341          prep4Grasp->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans13",speechnode3,StateNode::failureSignal));
01342          speechnode3->addTransition(new CompletionTrans("completiontrans19",postmachinefailure5));
01343          retrvObj->addTransition(new GrasperTrans("graspertrans1",verifyRetrv));
01344          verifyRetrv->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans14",getLoc,StateNode::successSignal));
01345          getLoc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans15",postmachinesuccess3,StateNode::successSignal));
01346          verifyRetrv->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans16",prep4Grasp,StateNode::failureSignal));
01347          prep4Grasp->addTransition(new CompletionTrans("completiontrans20",retrvObj));
01348         }
01349     
01350           private:
01351             DISALLOW_COPY_ASSIGN(ObjectManipRecovery);
01352         Kodu::VisualGripperMonitorTask* gTask;   // from $provide
01353         ShapeRoot grasperTarget;   // from $provide
01354         int objectTagId;   // from $provide
01355       };
01356   
01357       class LocalizeAgent : public StateNode {
01358        public:
01359         LocalizeAgent(const std::string &nodename="LocalizeAgent") : StateNode(nodename) {}
01360     
01361         class CreateLocalizeTask : public StateNode {
01362          public:
01363           CreateLocalizeTask(const std::string &nodename="CreateLocalizeTask") : StateNode(nodename) {}
01364           virtual void doStart() {
01365             Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
01366             Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
01367                 std::cout << "[" << getName() << "]\n";
01368                 std::cout << "Deleting task #" << currentTask->getTaskId() << std::endl;
01369                 std::cout << "creating a localization task ";
01370                 if (currentTask->getType() == Kodu::PT_VIS_LOCALIZATION) {
01371                   // remove old task
01372                   GeneralFncs::deletePtr(currentTask);
01373                   // make the localization task the new current task
01374                   // ********* temp fix
01375                   std::cout << " with max amount of stars ("
01376                             << theWorld->getStarConstellation().size() << ")";
01377                   currentTask = new Kodu::VisualLocalizationTask(theWorld->getStarConstellation(),
01378                                                                  theWorld->getStarConstellation().size());
01379                   // *********
01380                 } else {
01381                   // remove old task
01382                   GeneralFncs::deletePtr(currentTask);
01383                   // make the localization task the new current task
01384                   currentTask = new Kodu::VisualLocalizationTask(theWorld->getStarConstellation());
01385                 }
01386                 std::cout << "\n";
01387                 postParentCompletion();
01388           }
01389         };
01390     
01391         virtual void setup() {
01392          CreateLocalizeTask *createTask = new CreateLocalizeTask("createTask");
01393          addNode(createTask);
01394 
01395          startnode = createTask;
01396 
01397         }
01398       };
01399   
01400       class EndRecovery : public StateNode {
01401        public:
01402         EndRecovery(const std::string &nodename="EndRecovery") : StateNode(nodename) {}
01403         virtual void doStart() {
01404           bool &inRecovery = getAncestor<PerceptualMultiplexor>()->inRecovery;
01405             std::cout << "[" << getName() << "]: ending recovery...\n";
01406             inRecovery = false;
01407             postParentCompletion();
01408         }
01409       };
01410   
01411       virtual void setup() {
01412        PauseInterpretation *pause = new PauseInterpretation("pause");
01413        addNode(pause);
01414 
01415        StartRecovery *startRec = new StartRecovery("startRec");
01416        addNode(startRec);
01417 
01418        LocalizeAgent *locAgent = new LocalizeAgent("locAgent");
01419        addNode(locAgent);
01420 
01421        ObjectManipRecovery *manipRec = new ObjectManipRecovery("manipRec");
01422        addNode(manipRec);
01423 
01424        EndRecovery *endRec = new EndRecovery("endRec");
01425        addNode(endRec);
01426 
01427        SpeechNode *bumpRec = new SpeechNode("bumpRec","unhandled recovery for bump detection");
01428        addNode(bumpRec);
01429 
01430        SpeechNode *speechnode4 = new SpeechNode("speechnode4","unhandled failure recovery for manipulation");
01431        addNode(speechnode4);
01432 
01433        startnode = pause;
01434 
01435        pause->addTransition(new TimeOutTrans("timeouttrans1",pause,300));
01436        pause->addTransition(new CompletionTrans("completiontrans21",startRec));
01437        startRec->addTransition(new SignalTrans<Kodu::PerceptualTaskType_t>("signaltrans17",locAgent,Kodu::PT_VIS_LOCALIZATION));
01438        startRec->addTransition(new SignalTrans<Kodu::PerceptualTaskType_t>("signaltrans18",manipRec,Kodu::PT_GRIPPER_VIS_MONITOR));
01439        startRec->addTransition(new SignalTrans<Kodu::PerceptualTaskType_t>("signaltrans19",locAgent,Kodu::PT_VIS_NAV_ERR_MON));
01440        startRec->addTransition(new SignalTrans<Kodu::PerceptualTaskType_t>("signaltrans20",bumpRec,Kodu::PT_VIS_BUMP_DETECTION));
01441        manipRec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans21",endRec,StateNode::successSignal));
01442        manipRec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans22",speechnode4,StateNode::failureSignal));
01443        locAgent->addTransition(new CompletionTrans("completiontrans22",endRec));
01444       }
01445     };
01446 
01447     virtual void setup() {
01448      MultiplexorStart *startMP = new MultiplexorStart("startMP");
01449      addNode(startMP);
01450 
01451      MapBuilderTaskRunner *runMBTask = new MapBuilderTaskRunner("runMBTask");
01452      addNode(runMBTask);
01453 
01454      PilotTaskRunner *runPilotTask = new PilotTaskRunner("runPilotTask");
01455      addNode(runPilotTask);
01456 
01457      MultiplexorEnd *endMP = new MultiplexorEnd("endMP");
01458      addNode(endMP);
01459 
01460      FailureRecovery *recovery = new FailureRecovery("recovery");
01461      addNode(recovery);
01462 
01463      startnode = startMP;
01464 
01465      startMP->addTransition(new SignalTrans<MultiPlexorTransType_t>("signaltrans23",runMBTask,MPT_MAP_BUILDER));
01466      startMP->addTransition(new SignalTrans<MultiPlexorTransType_t>("signaltrans24",runPilotTask,MPT_PILOT));
01467      runMBTask->addTransition(new CompletionTrans("completiontrans23",endMP));
01468      runPilotTask->addTransition(new CompletionTrans("completiontrans24",endMP));
01469      endMP->addTransition(new CompletionTrans("completiontrans25",startMP));
01470      endMP->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans25",recovery,StateNode::failureSignal));
01471      recovery->addTransition(new CompletionTrans("completiontrans26",startMP));
01472     }
01473 
01474   private:
01475     DISALLOW_COPY_ASSIGN(PerceptualMultiplexor);
01476     Kodu::PerceptualTaskBase* currentTask;   // from $provide
01477     bool inRecovery;   // from $provide
01478   };
01479     
01480   class KoduConditionEvaluator : public StateNode {
01481    public:
01482     KoduConditionEvaluator(const std::string &nodename="KoduConditionEvaluator") : StateNode(nodename), cycleCount(0) {}
01483 
01484     //! Enables child rules with the once modifier enabled (who have already ran) to run (again)
01485     void resetChildRulesWithOnceEnabled(const unsigned int kParentRuleNumer) {
01486     Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
01487 
01488       Kodu::KoduPage* page = thisAgent->getCurrentPage();
01489       Kodu::KoduRule* rule = NULL;
01490       const int kRuleCount = page->getRuleCount();
01491       for (int childRule_i = kParentRuleNumer + 1; childRule_i < kRuleCount; childRule_i++) {
01492         // get rule# i
01493         rule = page->getRule(childRule_i);
01494         // check if this rule's parent is kParentRuleNumber
01495         // if it is, set actionCanRun flag to true
01496         // else break--this rule has no more child actions
01497         if (rule->getParentNumber() == kParentRuleNumer)
01498           rule->action->setAgentCanUsePrimitive(true);
01499         else
01500           break;
01501       }
01502     }
01503 
01504     virtual void doStart() {
01505     PerceptualMultiplexor* &multiplexorRef = getAncestor<KoduInterpreter>()->multiplexorRef;
01506     bool &needToHaltExecution = getAncestor<KoduInterpreter>()->needToHaltExecution;
01507     Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
01508     Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
01509     Kodu::KoduState* &kodustate = getAncestor<KoduInterpreter>()->kodustate;
01510 
01511       //std::cout << "\n=== [Kodu Condition Evaluator] ===\n";
01512       // check if the evaluator needs to halt execution
01513       if (needToHaltExecution) {
01514         //std::cout << "halting evaluator execution\n";
01515         postStateFailure();
01516         return;
01517       }
01518 
01519       // pointers to the current page, rule, condition, and action (respectively)
01520       // check if the current page is NULL
01521       Kodu::KoduPage* page = thisAgent->getCurrentPage();
01522       if ( page == NULL ) {
01523         //std::cerr << "!!! A NULL page was found.\n";
01524         postStateFailure();
01525         return;
01526       }
01527 
01528       if ( (cycleCount % 20) == 0 )
01529         std::cout << "GAME CYCLE #" << cycleCount << ", PAGE #" << page->getPageNumber() << "\n";
01530       ++cycleCount;
01531 
01532       // check if there are rules on this page
01533       const int numbOfRules = page->getRuleCount();
01534       if (numbOfRules == 0) {
01535         //std::cout << "Reached an empty page. Stopping interpreter...\n";
01536         postStateFailure();
01537         return;
01538       }
01539            
01540       Kodu::MotionCommand newMotionCmd = Kodu::MotionCommand();
01541 
01542       // iterate over all the rules of a page
01543       for (int i = 0; i < numbOfRules; i++) {
01544         Kodu::KoduRule* rule = page->getRuleInPos(i);
01545         if (rule == NULL) {
01546           std::cerr << "!!! A NULL rule was found. Tried accessing Page["
01547                     << (page->getPageNumber() - 1) << "], Rule[" << i << "]\n";
01548           postStateFailure();
01549           return;
01550         }
01551 
01552         const unsigned int kParentId = rule->getParentNumber();
01553         const unsigned int kRuleId = rule->getRuleNumber();
01554 
01555         // check if this rule has parent rule and if its condition evaluated true
01556         if (rule->isIndented() && page->getRule(kParentId)->condLastEvalResult == false) {
01557           rule->condLastEvalResult = false;
01558           continue;
01559         }
01560                 
01561         //cout << "kRuleId=" << kRuleId << " with parent=" << kParentId << endl;
01562 
01563         // point to the current condition and action
01564         Kodu::KoduCondition* cond = rule->condition;
01565         Kodu::KoduAction* act = rule->action;
01566 
01567         //Give condition and action access to state
01568         cond->setKoduState(*kodustate);
01569         act->setKoduState(*kodustate);
01570                 
01571         // check if the agent can evaluate the condition
01572         if (!cond->agentCanUsePrimitive()) {
01573           // std::cout << "cannot evaluate condition... continuing.\n";
01574           continue;
01575         }
01576                 
01577         // TODO (05/July/2013) Figure out how to cancel a running pilot request
01578         if (cond->evaluate(*theWorld) == true) {
01579           // resets the action has already ran flag
01580           // std::cout << "Condition Evaluated to true\n"; 
01581           if (rule->condLastEvalResult == false) {
01582             resetChildRulesWithOnceEnabled(kRuleId);
01583           }
01584                     
01585           // states condition evaluated true (no matter if action can execute or not)
01586           rule->setConditionEvalResult(true);
01587                     
01588           // check if the action can run
01589           if (!(act->agentCanUsePrimitive())) {
01590             std::cout << "Can't use this action\n";
01591             continue;
01592           }
01593           // check if this action:
01594           // 1) [implicit] can use the once modifier, and
01595           // 2) has the once modifier enabled
01596           if (act->onceModIsEnabled()) {
01597             act->setAgentCanUsePrimitive(false);
01598           }
01599                     
01600           // check the action type
01601           //std::cout << "checking actions: ";
01602           switch(act->getActionType()) {
01603             // Kodu Action Do Nothing
01604           case Kodu::KoduAction::AT_DO_NOTHING:
01605             {
01606               //      std::cout << "Do nothing.\n";
01607               break;
01608             }
01609 
01610             // Kodu Action Drop
01611           case Kodu::KoduAction::AT_DROP:
01612             {
01613               std::cout << "drop; ";
01614               // a drop action can only execute if the agent is:
01615               // 1) holding an object,
01616               // 2) not executing another manipulation action
01617               // 3) not executing a motion action
01618               if (thisAgent->isHoldingAnObject() && !thisAgent->isExecutingManipAction()
01619                   && !thisAgent->isExecutingMotionAction())
01620                 {
01621                   std::cout << "Agent wants to drop item.\n";
01622                   // signal that a drop action is executing (prevents any motion or 
01623                   // other manipulation actions from executing)
01624                   thisAgent->signalDropActionStart();
01625                 }
01626               break;
01627             }
01628 
01629             // Kodu Action Grab
01630           case Kodu::KoduAction::AT_GRAB:
01631             {
01632               std::cout << "grab; ";
01633               // a grab action can only execute if the agent is:
01634               // 1) not executing a motion action,
01635               // 2) not executing another manipulation action
01636               // 3) the target shape is valid
01637               // 4) the target shapes is not the same shape in the gripper
01638               if (!thisAgent->isExecutingMotionAction()
01639                   && !thisAgent->isExecutingManipAction())
01640                 {
01641                   // get the target object
01642                   ShapeRoot targetObject =
01643                     static_cast<Kodu::KoduActionGrab*>(act)->getTargetObject();
01644                                 
01645                   if (targetObject.isValid() && targetObject != thisAgent->gripperObject)
01646                     {
01647                       // signal that a grab action wants to execute (prevents any motion
01648                       // or other manipulation actions from executing)
01649                       thisAgent->signalGrabActionStart();
01650                       thisAgent->gripperObject = targetObject;
01651                       std::cout << "Selected an object " << targetObject
01652                                 << " to be grabbed.\n";
01653                     }
01654                 }
01655               break;
01656             }
01657 
01658             // Kodu Action Give
01659           case Kodu::KoduAction::AT_GIVE:
01660             {
01661               std::cout << "give; ";
01662               if (!thisAgent->isExecutingMotionAction() && !thisAgent->isExecutingManipAction()
01663                   && thisAgent->isHoldingAnObject()) {
01664                                 
01665                 Kodu::KoduActionGive* giveAction = static_cast<Kodu::KoduActionGive*>(act);
01666                 ShapeRoot targetAgent = giveAction->getTargetObject();
01667                 // Make sure the given target is an agentdata, and then set it as the target and signal a give
01668                 if (targetAgent != Kodu::ObjectKeeper::invalidObject && targetAgent->isType(DualCoding::agentDataType)
01669                     && targetAgent.isValid()) {
01670                   thisAgent->giveTargetObject = ShapeRootType(targetAgent,AgentData);
01671                   thisAgent->signalGiveActionStart();
01672                 }
01673               }
01674               break;
01675             }
01676 
01677             // Kodu Action Motion
01678           case Kodu::KoduAction::AT_MOTION:
01679             {
01680               if (! newMotionCmd.isValid() ) {
01681                 newMotionCmd = static_cast<Kodu::KoduActionMotion*>(act)->getMotionCommand();
01682                 // std::cout << "motion: " << newMotionCmd << std::endl;
01683                 if ( thisAgent->currMotionCmd != newMotionCmd ) {
01684                   cout << "agent had " << thisAgent->currMotionCmd << " newMotionCmd=" << newMotionCmd << endl;
01685                   if ( thisAgent->currMotionCmd.isValid() ) {
01686     MotionActionRunner* &motionActRef = getAncestor<KoduInterpreter>()->motionActRef;
01687                     motionActRef->stop();
01688                   }
01689 
01690                   // TODO
01691                   // should the interpreter check if the robot is executing a 
01692                   // manip action here or in the action runner node?
01693                   
01694                   std::cout << "Adding motion command " << newMotionCmd << std::endl;
01695                   thisAgent->setMotionCommand(newMotionCmd);
01696                 } else if ( newMotionCmd.isDriven() ) { //Update Velocities
01697                   std::cout << "Adjusting drive velocity" << std::endl; 
01698                   float xvel = kodustate->joysticky;
01699                   float avel = kodustate->joystickx;
01700                   float speed = 125.0;
01701                   float angvel = M_PI/8.0;
01702                   pilot->changeVelocity(speed*xvel, 0.0, angvel*avel);
01703                 }
01704               } 
01705               break;
01706             }
01707 
01708             // Kodu Action Page Switch
01709           case Kodu::KoduAction::AT_PAGE_SWITCH:
01710             {
01711               std::cout << "page switch; ";
01712               // get the new requested page number (agent will switch to it immediately)
01713               if (!thisAgent->hasNewPageNumber()) {
01714                 thisAgent->newReqdPage =
01715                   static_cast<Kodu::KoduActionPageSwitch*>(act)->getPageNumber();
01716                 std::cout << "Switching to page " << thisAgent->newReqdPage << std::endl;
01717               }
01718               break;
01719             }
01720 
01721             // Kodu Action Play
01722           case Kodu::KoduAction::AT_PLAY:
01723             {
01724               std::cout << "play; ";
01725               // get the sound file
01726               std::string soundFile =
01727                 static_cast<Kodu::KoduActionPlay*>(act)->getSoundFile();
01728               // add the sound file to the play queue
01729               thisAgent->playQueue.push(soundFile);
01730               std::cout << "Added the sound file \"" << soundFile << "\".\n";
01731               break;
01732             }
01733 
01734             // Kodu Action Say
01735           case Kodu::KoduAction::AT_SAY:
01736             {
01737               std::cout << "say; ";
01738               // make sure the agent does not have anything to say first
01739               if (!thisAgent->hasTextToSay()) {
01740                 // give the string to the agent
01741                 thisAgent->stringToSpeak =
01742                   static_cast<Kodu::KoduActionSay*>(act)->getStringToSpeak();
01743                 std::cout << "Added literal string: \""
01744                           << thisAgent->stringToSpeak << "\".\n";
01745               }
01746               break;
01747             }
01748 
01749             // Kodu Action Score
01750           case Kodu::KoduAction::AT_SCORING:
01751             {
01752               std::cout << "score; ";
01753               // add the score change to the score queue
01754               thisAgent->scoreQueue.push(
01755                                          static_cast<Kodu::KoduActionScore*>(act)->getScoreChange());
01756               std::cout << "Added a score action type.\n";
01757               break;
01758             }
01759 
01760           default:
01761             {
01762               std::cerr << "Unrecognized action: \""
01763                         << act->getPrimitiveType() << "\"\n";
01764               break;
01765             }
01766           }
01767           // check if the agent needs to switch pages
01768           if (thisAgent->hasNewPageNumber())
01769             break;
01770         } else {
01771           // states rule evaluated false
01772           rule->setConditionEvalResult(false);
01773           // check the condition type...
01774           // if it is not a timer condition, then set the "action can run" flag to true
01775           if (!cond->getConditionType() != Kodu::KoduCondition::CT_TIMER) {
01776             act->setAgentCanUsePrimitive(true);
01777           }
01778           /**
01779            * check if the condition is type KoduConditionBump
01780            * if so, stop evaluating the condition, and create a perceptual task for it.
01781            * once the robot is close enough to detect a bump, the task will visually detect
01782            * bump. if the detection was successful, the agent would be able to evaluate the
01783            * rule again. if not, the rule is disabled for some time (temp fix)
01784            **/
01785           if (cond->getConditionType() == Kodu::KoduCondition::CT_BUMP
01786               && !thisAgent->isExecutingManipAction())
01787             {
01788               thisAgent->ptasks.push(new Kodu::VisualBumpDetectionTask(static_cast<Kodu::KoduConditionBump*>(cond), theWorld));
01789             }
01790         }
01791         // invalidate object
01792         Kodu::ObjectKeeper::isValid = false;
01793         cond = NULL;
01794         act = NULL;
01795       }
01796       kodustate->clearUtterances();
01797 
01798       if ( thisAgent->currMotionCmd.isValid() && !newMotionCmd.isValid() ) {
01799         cout << "Current motion no longer supported by rule: aborting." << endl;
01800         pilot->pilotAbort();
01801       }
01802 
01803       //PRINT_ATTRS("P-Task queue empty?", thisAgent->ptasks.empty());
01804       //PRINT_ATTRS("Multiplexor active?", multiplexorRef->isActive());
01805       if (!thisAgent->ptasks.empty() && !multiplexorRef->isActive()){
01806         //std::cout << "starting multiplexor...\n";
01807         multiplexorRef->start();
01808       }
01809       postStateCompletion();
01810     }
01811 
01812     unsigned int cycleCount;   // from $provide
01813   };
01814 
01815   //! Kodu Action Runner
01816   class KoduActionRunner : public StateNode {
01817    public:
01818     KoduActionRunner(const std::string &nodename="KoduActionRunner") : StateNode(nodename) {}
01819     virtual void doStart() {
01820       Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
01821       DropActionRunner* &dropActRef = getAncestor<KoduInterpreter>()->dropActRef;
01822       GrabActionRunner* &grabActRef = getAncestor<KoduInterpreter>()->grabActRef;
01823       GiveActionRunner* &giveActRef = getAncestor<KoduInterpreter>()->giveActRef;
01824       ReceiveActionRunner* &recvActRef = getAncestor<KoduInterpreter>()->recvActRef;
01825       MotionActionRunner* &motionActRef = getAncestor<KoduInterpreter>()->motionActRef;
01826       PageSwitchActionRunner* &pageSwitchActRef = getAncestor<KoduInterpreter>()->pageSwitchActRef;
01827       PlayActuator* &playActRef = getAncestor<KoduInterpreter>()->playActRef;
01828       SayActuator* &sayActRef = getAncestor<KoduInterpreter>()->sayActRef;
01829       ScoreActuator* &scoreActRef = getAncestor<KoduInterpreter>()->scoreActRef;
01830           
01831       //std::cout << "=== [Kodu Action Runner] ===\n";
01832       if (thisAgent->wantsToDropObject())
01833         dropActRef->start();
01834   
01835       if (thisAgent->wantsToGrabObject())
01836         grabActRef->start();
01837   
01838       if (thisAgent->wantsToGiveObject())
01839         giveActRef->start();
01840   
01841       if (thisAgent->agentIsReceiving)
01842         recvActRef->start();
01843   
01844       if (thisAgent->currMotionCmd.isValid() && !thisAgent->hasNewPageNumber()
01845           && !thisAgent->isExecutingManipAction())
01846         motionActRef->start();
01847   
01848       if (thisAgent->hasSoundsToPlay())
01849         playActRef->start();
01850           
01851       if (thisAgent->hasTextToSay())
01852         sayActRef->start();
01853               
01854       if (thisAgent->hasNewScoreChanges())
01855         scoreActRef->start();
01856   
01857       if (thisAgent->hasNewPageNumber())
01858         pageSwitchActRef->start();
01859     }
01860   };
01861 
01862   // ================================ Action Handlers ========================================= //
01863 
01864   // DropActionRunner
01865 
01866   class DropActionRunner : public StateNode {
01867    public:
01868     DropActionRunner(const std::string &nodename="DropActionRunner") : StateNode(nodename), gTask(NULL) {}
01869 
01870     ~DropActionRunner() {
01871       gTask = NULL;
01872     }
01873 
01874     class DropActionStart : public StateNode {
01875      public:
01876       DropActionStart(const std::string &nodename="DropActionStart") : StateNode(nodename) {}
01877       virtual void doStart() {
01878           std::cout << "[" << getName() << "]: initiating drop action.\n";
01879           std::cout << "Searching for the gripper monitor task.\n";
01880           // locate the gripper monitor task
01881         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
01882         Kodu::VisualGripperMonitorTask* &gTask = getAncestor<DropActionRunner>()->gTask;
01883           gTask = NULL;
01884           unsigned int tasksToExecute = thisAgent->ptasks.size();
01885           while (tasksToExecute > 0) {
01886             if (thisAgent->ptasks.front()->getType() == Kodu::PT_GRIPPER_VIS_MONITOR) {
01887               gTask = static_cast<Kodu::VisualGripperMonitorTask*>(thisAgent->ptasks.front());
01888               thisAgent->ptasks.pop();
01889             } else {
01890               thisAgent->ptasks.push(thisAgent->ptasks.front());
01891               thisAgent->ptasks.pop();
01892             }
01893             tasksToExecute--;
01894           }
01895     
01896           // should never happen, but just in case...
01897           if (gTask == NULL) {
01898             std::cout << "THIS IS A SERIOUS ERROR!!!---the gripper task should not be null!\n";
01899             postStateFailure();
01900             return;
01901           }
01902           std::cout << "GripMonTask id = " << gTask->getTaskId() << std::endl;
01903           postStateCompletion();
01904       }
01905     };
01906 
01907     class ExecuteDrop : public StateNode {
01908      public:
01909       ExecuteDrop(const std::string &nodename="ExecuteDrop") : StateNode(nodename), objFutureEgoPos(), kReverseDist(120.0f) {}
01910   
01911       class DropItem : public ArmNode {
01912        public:
01913         DropItem(const std::string &nodename="DropItem") : ArmNode(nodename) {}
01914         virtual void doStart() {
01915               std::cout << "[" << getName() << "]: releasing the item.\n";
01916               // open the gripper --- this value is assuming the cylinder's diameter < 120mm
01917               getMC()->openGripper(0.45f);
01918         }
01919       };
01920   
01921       class LookAtObjectAprilTag : public MapBuilderNode {
01922        public:
01923         LookAtObjectAprilTag(const std::string &nodename="LookAtObjectAprilTag") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
01924         virtual void doStart() {
01925               std::cout << "[" << getName() << "]: looking for released object's april tag\n";
01926           Kodu::VisualGripperMonitorTask* &gTask = getAncestor<DropActionRunner>()->gTask;
01927               // create a mapbuilder request to locate the april tag one more time.
01928               // will be used to identify the correct object if more than one are in view
01929               // once the object is released.
01930               mapreq.setAprilTagFamily();
01931               NEW_SHAPE(gazePoint, PointData, new PointData(localShS, gTask->getTagCentroid()));
01932               mapreq.searchArea = gazePoint;
01933               mapreq.clearLocal = true;
01934         }
01935       };
01936   
01937       class RecordAprilTagPos : public StateNode {
01938        public:
01939         RecordAprilTagPos(const std::string &nodename="RecordAprilTagPos") : StateNode(nodename) {}
01940         virtual void doStart() {
01941               std::cout << "[" << getName() << "]: attempting to record april tag's position.\n";
01942               // find the object's april tag position and log it
01943           Kodu::VisualGripperMonitorTask* &gTask = getAncestor<DropActionRunner>()->gTask;
01944               NEW_SHAPE(objTag, AprilTagData, find_if<AprilTagData>(localShS,
01945                                                                     Kodu::HasAprilTagID(gTask->getTagId())));
01946               // check if the shape is valid (that means the tag was found)
01947               if (objTag.isValid()) {
01948           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
01949           Point &objFutureEgoPos = getAncestor<ExecuteDrop>()->objFutureEgoPos;
01950           float &kReverseDist = getAncestor<ExecuteDrop>()->kReverseDist;
01951                 std::cout << "found the tag @ " << objTag->getCentroid() << "; logging it\n";
01952                 const Point& kTagCurrPos = objTag->getCentroid();
01953                 const float kCylHeight
01954                   = ShapeRootTypeConst(thisAgent->gripperObject, CylinderData)->getHeight();
01955                 objFutureEgoPos = Point(kTagCurrPos.coordX() + kReverseDist, kTagCurrPos.coordY(),
01956                                         (kCylHeight / 2.0f), egocentric);
01957                 postStateSuccess();
01958               }
01959               // else, report an error... lighting could be an issue if this did happen though
01960               else {
01961                 std::cout << "DID NOT FIND THE APRIL TAG!!!\n";
01962                 postStateFailure();
01963               }
01964         }
01965       };
01966   
01967       class ReverseBody : public PilotNode {
01968        public:
01969         ReverseBody(const std::string &nodename="ReverseBody") : PilotNode(nodename,PilotTypes::walk) {}
01970         virtual void doStart() {
01971               std::cout << "[" << getName() << "]: reversing body by ";
01972           float &kReverseDist = getAncestor<ExecuteDrop>()->kReverseDist;
01973               pilotreq.dx = -1.0f * kReverseDist;
01974               std::cout << pilotreq.dx << "mm\n";
01975         }
01976       };
01977   
01978       class LocalizeAgent : public PilotNode {
01979        public:
01980         LocalizeAgent(const std::string &nodename, unsigned int _numbOfStarsRequested) : PilotNode(nodename), numbOfStarsRequested(_numbOfStarsRequested) {}
01981         unsigned int numbOfStarsRequested;  // cache the constructor's parameter
01982         virtual void doStart() {
01983               std::cout << "[" << getName() << "]: attempting to localize\n";
01984           Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
01985               // create a localization task, and execute it before adding the shape in the world
01986               Kodu::VisualLocalizationTask localizeTask(theWorld->getStarConstellation(),
01987                                                         numbOfStarsRequested);
01988               pilotreq = localizeTask.getPilotRequest();
01989         }
01990       };
01991   
01992       class RepositionReleasedObject : public StateNode {
01993        public:
01994         RepositionReleasedObject(const std::string &nodename="RepositionReleasedObject") : StateNode(nodename) {}
01995     
01996         class LookForObjectAgain : public MapBuilderNode {
01997          public:
01998           LookForObjectAgain(const std::string &nodename="LookForObjectAgain") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
01999           virtual void doStart() {
02000                   std::cout << "[" << getName() << "]: looking for object again\n";
02001             Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02002             Point &objFutureEgoPos = getAncestor<ExecuteDrop>()->objFutureEgoPos;
02003                   // create the point the agent should look at
02004                   NEW_SHAPE(gazePoint, PointData, new PointData(localShS, objFutureEgoPos));
02005                   const float kCylRadius
02006                     = ShapeRootTypeConst(thisAgent->gripperObject, CylinderData)->getRadius();
02007         
02008                   // construct the mapbuilder request
02009                   mapreq.addAttributes(thisAgent->gripperObject);
02010                   mapreq.searchArea = gazePoint;
02011                   mapreq.maxDist = objFutureEgoPos.xyNorm() + (kCylRadius * 2.0f);
02012                   mapreq.pursueShapes = true;
02013                   mapreq.clearLocal = true;
02014           }
02015         };
02016     
02017         class RepositionObject : public StateNode {
02018          public:
02019           RepositionObject(const std::string &nodename="RepositionObject") : StateNode(nodename) {}
02020           virtual void doStart() {
02021                   std::cout << "[" << getName() << "]: attempting to 'reposition' the object\n";
02022             Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02023             Point &objFutureEgoPos = getAncestor<ExecuteDrop>()->objFutureEgoPos;
02024                   // get all the shapes that are the same type as the object in the gripper
02025                   ShapeType_t objType = thisAgent->gripperObject->getType();
02026                   std::vector<ShapeRoot> localShapes(localShS.allShapes(objType));
02027         
02028                   // should not ever happen, but just in case...
02029                   if (localShapes.empty()) {
02030                     std::cout << "THIS IS A SERIOUS ERROR!\n";
02031                     postStateFailure();
02032                     return;
02033                   }
02034         
02035                   // get the closest object to the precalculated point
02036                   ShapeRoot closestMatch = Kodu::getClosestObjectToPoint(localShapes, objFutureEgoPos);
02037                   // apply tranformation to closest match (egocentric --> allocentric)
02038                   Point closestMatchPt(closestMatch->getCentroid());
02039                   closestMatchPt.applyTransform(VRmixin::mapBuilder->localToWorldMatrix, allocentric);
02040                   std::cout << "Repositioning the object to from "
02041                             << thisAgent->gripperObject->getCentroid() << " to "
02042                             << closestMatchPt << std::endl;
02043                             
02044                   // reposition the gripper object
02045                   thisAgent->gripperObject->setPosition(closestMatchPt);
02046                   thisAgent->gripperObject.setInvalid();
02047                   postStateSuccess();
02048           }
02049         };
02050     
02051         virtual void setup() {
02052          LookForObjectAgain *lookAgain = new LookForObjectAgain("lookAgain");
02053          addNode(lookAgain);
02054 
02055          RepositionObject *reposition = new RepositionObject("reposition");
02056          addNode(reposition);
02057 
02058          PostMachineSuccess *postmachinesuccess4 = new PostMachineSuccess("postmachinesuccess4");
02059          addNode(postmachinesuccess4);
02060 
02061          SpeechNode *speechnode5 = new SpeechNode("speechnode5","could not find any local shapes");
02062          addNode(speechnode5);
02063 
02064          PostMachineFailure *postmachinefailure6 = new PostMachineFailure("postmachinefailure6");
02065          addNode(postmachinefailure6);
02066 
02067          startnode = lookAgain;
02068 
02069          lookAgain->addTransition(new CompletionTrans("completiontrans27",reposition));
02070          reposition->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans26",postmachinesuccess4,StateNode::successSignal));
02071          reposition->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans27",speechnode5,StateNode::failureSignal));
02072          speechnode5->addTransition(new CompletionTrans("completiontrans28",postmachinefailure6));
02073         }
02074       };
02075   
02076       virtual void setup() {
02077        DropItem *drop = new DropItem("drop");
02078        addNode(drop);
02079 
02080        LookAtObjectAprilTag *lookAtTag = new LookAtObjectAprilTag("lookAtTag");
02081        addNode(lookAtTag);
02082 
02083        RecordAprilTagPos *recordATPos = new RecordAprilTagPos("recordATPos");
02084        addNode(recordATPos);
02085 
02086        ReverseBody *reverse = new ReverseBody("reverse");
02087        addNode(reverse);
02088 
02089        ParkArm *parkArm = new ParkArm("parkArm");
02090        addNode(parkArm);
02091 
02092        LocalizeAgent *localize2 = new LocalizeAgent("localize2",Kodu::VisualLocalizationTask::kMinStarsRequiredToLocalize);
02093        addNode(localize2);
02094 
02095        LocalizeAgent *localize3 = new LocalizeAgent("localize3",Kodu::VisualLocalizationTask::kMinStarsRequiredToLocalize + 1);
02096        addNode(localize3);
02097 
02098        RepositionReleasedObject *attempRepos = new RepositionReleasedObject("attempRepos");
02099        addNode(attempRepos);
02100 
02101        SpeechNode *speechnode6 = new SpeechNode("speechnode6","error occurred while dropping object");
02102        addNode(speechnode6);
02103 
02104        PostMachineFailure *postmachinefailure7 = new PostMachineFailure("postmachinefailure7");
02105        addNode(postmachinefailure7);
02106 
02107        SpeechNode *speechnode7 = new SpeechNode("speechnode7","could not record the tag position");
02108        addNode(speechnode7);
02109 
02110        PostMachineFailure *postmachinefailure8 = new PostMachineFailure("postmachinefailure8");
02111        addNode(postmachinefailure8);
02112 
02113        StateNode *statenode2 = new StateNode("statenode2");
02114        addNode(statenode2);
02115 
02116        SpeechNode *speechnode8 = new SpeechNode("speechnode8","localize failed");
02117        addNode(speechnode8);
02118 
02119        PostMachineFailure *postmachinefailure9 = new PostMachineFailure("postmachinefailure9");
02120        addNode(postmachinefailure9);
02121 
02122        PostMachineSuccess *postmachinesuccess5 = new PostMachineSuccess("postmachinesuccess5");
02123        addNode(postmachinesuccess5);
02124 
02125        PostMachineFailure *postmachinefailure10 = new PostMachineFailure("postmachinefailure10");
02126        addNode(postmachinefailure10);
02127 
02128        startnode = drop;
02129 
02130        drop->addTransition(new CompletionTrans("completiontrans29",lookAtTag));
02131        lookAtTag->addTransition(new CompletionTrans("completiontrans30",recordATPos));
02132        recordATPos->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans28",reverse,StateNode::successSignal));
02133 
02134        CompletionTrans *completiontrans31 = new CompletionTrans("completiontrans31",localize2);
02135        completiontrans31->addDestination(parkArm);
02136        reverse->addTransition(completiontrans31);
02137 
02138        drop->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans29",speechnode6,StateNode::failureSignal));
02139        speechnode6->addTransition(new CompletionTrans("completiontrans32",postmachinefailure7));
02140        recordATPos->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans30",speechnode7,StateNode::failureSignal));
02141        speechnode7->addTransition(new CompletionTrans("completiontrans33",postmachinefailure8));
02142        localize2->addTransition(new PilotTrans("pilottrans3",attempRepos,PilotTypes::noError));
02143        localize2->addTransition(new PilotTrans("pilottrans4",localize3,PilotTypes::cantLocalize));
02144        parkArm->addTransition(new CompletionTrans("completiontrans34",statenode2));
02145        localize3->addTransition(new PilotTrans("pilottrans5",attempRepos,PilotTypes::noError));
02146        localize3->addTransition(new PilotTrans("pilottrans6",speechnode8,PilotTypes::cantLocalize));
02147        speechnode8->addTransition(new CompletionTrans("completiontrans35",postmachinefailure9));
02148        attempRepos->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans31",postmachinesuccess5,StateNode::successSignal));
02149        attempRepos->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans32",postmachinefailure10,StateNode::failureSignal));
02150       }
02151       Point objFutureEgoPos;   // from $provide
02152       float kReverseDist;   // from $provide
02153     };
02154 
02155     class DropActionEnd : public StateNode {
02156      public:
02157       DropActionEnd(const std::string &nodename="DropActionEnd") : StateNode(nodename) {}
02158       virtual void doStart() {
02159         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02160         Kodu::VisualGripperMonitorTask* &gTask = getAncestor<DropActionRunner>()->gTask;
02161           std::cout << "[" << getName() << "]: finishing up drop action\n";
02162           // delete the april tag's created during localization
02163           std::vector<ShapeRoot> tagVec = worldShS.allShapes(aprilTagDataType);
02164           worldShS.deleteShapes(tagVec);
02165           // delete the gripper monitor task
02166           GeneralFncs::deletePtr(gTask);
02167           // states the object is no longer in the gripper, and release the hold on
02168           // executing manipulation actions (and implicitly motion too)
02169           thisAgent->manipulationComplete();
02170           postStateCompletion();
02171       }
02172     };
02173 
02174     virtual void setup() {
02175      DropActionStart *dStart = new DropActionStart("dStart");
02176      addNode(dStart);
02177 
02178      ExecuteDrop *exec = new ExecuteDrop("exec");
02179      addNode(exec);
02180 
02181      DropActionEnd *dEnd = new DropActionEnd("dEnd");
02182      addNode(dEnd);
02183 
02184      SpeechNode *speechnode9 = new SpeechNode("speechnode9","failure during drop execution");
02185      addNode(speechnode9);
02186 
02187      PostMachineCompletion *postmachinecompletion3 = new PostMachineCompletion("postmachinecompletion3");
02188      addNode(postmachinecompletion3);
02189 
02190      startnode = dStart;
02191 
02192      dStart->addTransition(new CompletionTrans("completiontrans36",exec));
02193      exec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans33",dEnd,StateNode::successSignal));
02194      exec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans34",speechnode9,StateNode::failureSignal));
02195      speechnode9->addTransition(new CompletionTrans("completiontrans37",dEnd));
02196      dEnd->addTransition(new CompletionTrans("completiontrans38",postmachinecompletion3));
02197     }
02198 
02199   private:
02200     DISALLOW_COPY_ASSIGN(DropActionRunner);
02201     Kodu::VisualGripperMonitorTask* gTask;   // from $provide
02202   };
02203 
02204   // GrabActionRunner
02205 
02206   class GrabActionRunner : public StateNode {
02207    public:
02208     GrabActionRunner(const std::string &nodename="GrabActionRunner") : StateNode(nodename), objectTagId(-1) {}
02209         
02210     class GrabActionStart : public StateNode {
02211      public:
02212       GrabActionStart(const std::string &nodename="GrabActionStart") : StateNode(nodename) {}
02213   
02214         // first check if the agent is near enough to the object
02215       class IsObjectNear : public StateNode {
02216        public:
02217         IsObjectNear(const std::string &nodename="IsObjectNear") : StateNode(nodename) {}
02218         virtual void doStart() {
02219           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02220               std::cout << "[" << getName() << "]: is agent near object? ";
02221               if (Kodu::distanceInBetweenAgentAndObject(thisAgent->gripperObject)
02222                   < Kodu::KoduConditionBump::kMaxDistanceAwayToSenseBump)
02223                 {
02224                   std::cout << "yes, it is!\n";
02225                   postStateSuccess();
02226                 } else {
02227                 std::cout << "no, it is not. making gripperObject reference invalid.\n";
02228                 thisAgent->gripperObject = ShapeRoot();
02229                 postStateFailure();
02230               }
02231         }
02232       };
02233   
02234         /**
02235          * ASSUMPTIONS:
02236          *  - the object was bumped before this action was executed.
02237          **/
02238       class GetTagId : public StateNode {
02239        public:
02240         GetTagId(const std::string &nodename="GetTagId") : StateNode(nodename) {}
02241         virtual void doStart() {
02242               std::cout << "[" << getName() << "]: getting tag id. (id = ";
02243           Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
02244           int &objectTagId = getAncestor<GrabActionRunner>()->objectTagId;
02245               // if the {world shape id, tag id} pair exists, log the tag id
02246               if (theWorld->shapeTagPairExists(theWorld->thisAgent.gripperObject.getId())) {
02247                 objectTagId = theWorld->getTagIdForShape(theWorld->thisAgent.gripperObject.getId());
02248               }
02249               std::cout << objectTagId << ").\n";
02250               std::cout << "target id = " << theWorld->thisAgent.gripperObject.getId() << std::endl;
02251               postStateCompletion();
02252         }
02253       };
02254   
02255       virtual void setup() {
02256        IsObjectNear *isNear = new IsObjectNear("isNear");
02257        addNode(isNear);
02258 
02259        GetTagId *getTagId = new GetTagId("getTagId");
02260        addNode(getTagId);
02261 
02262        PostMachineSuccess *postmachinesuccess6 = new PostMachineSuccess("postmachinesuccess6");
02263        addNode(postmachinesuccess6);
02264 
02265        PostMachineFailure *postmachinefailure11 = new PostMachineFailure("postmachinefailure11");
02266        addNode(postmachinefailure11);
02267 
02268        startnode = isNear;
02269 
02270        isNear->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans35",getTagId,StateNode::successSignal));
02271        getTagId->addTransition(new CompletionTrans("completiontrans39",postmachinesuccess6));
02272        isNear->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans36",postmachinefailure11,StateNode::failureSignal));
02273       }
02274     };
02275 
02276     /**
02277      * Positions the body so that the robot can grab the object
02278      * ASSUMPTIONS:
02279      *  - the item is graspable, and there is nothing in between the agent and the target object
02280      **/
02281     class PrepareBody : public StateNode {
02282      public:
02283       PrepareBody(const std::string &nodename="PrepareBody") : StateNode(nodename) {}
02284   
02285         /**
02286          * Face the object.
02287          * ASSUMPTIONS:
02288          * - the arm is parked
02289          **/
02290       class FaceObject : public PilotNode {
02291        public:
02292         FaceObject(const std::string &nodename="FaceObject") : PilotNode(nodename,PilotTypes::walk) {}
02293         virtual void doStart() {
02294           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02295               std::cout << "[" << getName() << "]: turning towards object\n";
02296               pilotreq.da = Kodu::bearingFromAgentToObject(thisAgent->gripperObject);
02297         }
02298       };
02299   
02300         /**
02301          * Reverse the body so the arm has enough space to swivel without hitting the object
02302          * ASSUMPTIONS:
02303          *  - the arm's length is less than or equal to kArmMaxLength
02304          **/
02305       class ReverseBody : public PilotNode {
02306        public:
02307         ReverseBody(const std::string &nodename="ReverseBody") : PilotNode(nodename,PilotTypes::walk) {}
02308         virtual void doStart() {
02309           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02310               std::cout << "[" << getName() << "]: reversing body ";
02311               float const kArmMaxLength = 210.0f;
02312               float distInBtwObjects = Kodu::distanceInBetweenAgentAndObject(thisAgent->gripperObject);
02313               pilotreq.dx = -1.0f * kArmMaxLength;
02314               std::cout << std::fabs(pilotreq.dx) << "mm (" << kArmMaxLength << " + "
02315                         << distInBtwObjects << ")\n";
02316         }
02317       };
02318   
02319       virtual void setup() {
02320        FaceObject *face = new FaceObject("face");
02321        addNode(face);
02322 
02323        ReverseBody *rvrs = new ReverseBody("rvrs");
02324        addNode(rvrs);
02325 
02326        PostMachineCompletion *postmachinecompletion4 = new PostMachineCompletion("postmachinecompletion4");
02327        addNode(postmachinecompletion4);
02328 
02329        startnode = face;
02330 
02331        face->addTransition(new CompletionTrans("completiontrans40",rvrs));
02332        rvrs->addTransition(new CompletionTrans("completiontrans41",postmachinecompletion4));
02333       }
02334     };
02335 
02336     class ExecuteGrabAction : public StateNode {
02337      public:
02338       ExecuteGrabAction(const std::string &nodename="ExecuteGrabAction") : StateNode(nodename) {}
02339               
02340         /**
02341          * Attempt to grab the object
02342          * ASSUMPTIONS:
02343          *  - the robot's arm will not hit the object (the PrepareBody state machine worked)
02344          **/
02345       class GrabObject : public GrasperNode {
02346        public:
02347         GrabObject(const std::string &nodename="GrabObject") : GrasperNode(nodename,GrasperRequest::grasp) {}
02348         virtual void doStart() {
02349           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02350               std::cout << "[" << getName() << "]: attempting to grab " << thisAgent->gripperObject
02351                         << std::endl;
02352               graspreq.object = thisAgent->gripperObject;
02353         }
02354       };
02355   
02356       class VerifyObjectWasGrabbed : public StateNode {
02357        public:
02358         VerifyObjectWasGrabbed(const std::string &nodename="VerifyObjectWasGrabbed") : StateNode(nodename) {}
02359     
02360         class LookAtTheGripper : public MapBuilderNode {
02361          public:
02362           LookAtTheGripper(const std::string &nodename="LookAtTheGripper") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
02363           virtual void doStart() {
02364                   std::cout << "[" << getName() << "]: looking at tag\n";
02365                   mapreq.clearLocal = true;
02366                   // search for an april tag
02367                   mapreq.setAprilTagFamily();
02368                   // look at the gripper
02369                   fmat::Column<3> gripperLoc;
02370         #if defined(TGT_IS_CALLIOPE2)
02371                   gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
02372         #endif
02373                   NEW_SHAPE(gazePoint, PointData, new PointData(localShS,
02374                                                                 Point(gripperLoc[0], gripperLoc[1], gripperLoc[2], egocentric)));
02375                   mapreq.searchArea = gazePoint;
02376           }
02377         };
02378     
02379         class VerifyObjectInGripper : public StateNode {
02380          public:
02381           VerifyObjectInGripper(const std::string &nodename="VerifyObjectInGripper") : StateNode(nodename) {}
02382           virtual void doStart() {
02383             Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
02384             Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02385                   std::cout << "[" << getName() << "]: verifying object was grabbed.\n";
02386                             
02387                   // check if a particular tag was seen
02388                   NEW_SHAPE(tag, AprilTagData, 
02389                             find_if<AprilTagData>(localShS,
02390                                                   Kodu::HasAprilTagID(theWorld->getTagIdForShape(thisAgent->gripperObject.getId()))));
02391                             
02392                   // if the reference is not valid, post a failure
02393                   if (!tag.isValid()) {
02394                     std::cout << "THE TAG WAS NOT SEEN (FAILURE)!!!\n";
02395                     postStateFailure();
02396                     return;
02397                   }
02398         
02399                   // ********** temp fix
02400                   // 
02401                   // get the gripper location
02402                   fmat::Column<3> gripperLoc;
02403         #if defined(TGT_IS_CALLIOPE2)
02404                   gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
02405         #endif
02406                   Point gripperPoint(gripperLoc[0], gripperLoc[1], gripperLoc[2], egocentric);
02407         
02408                   // check if the object is in the gripper
02409                   if (tag->getCentroid().xyDistanceFrom(gripperPoint)
02410                       < (Kodu::objectRadius(thisAgent->gripperObject) * 1.25f))
02411                     {
02412                       std::cout << "successfully grabbed object\n";
02413                       postStateSuccess();
02414                     } else {
02415                     std::cout << "failed to grab object... dist = "
02416                               << tag->getCentroid().xyDistanceFrom(gripperPoint)
02417                               << "mm; reattempting.\n";
02418                     postStateFailure();
02419                   }
02420                   // ***********
02421           }
02422         };
02423     
02424         virtual void setup() {
02425          LookAtTheGripper *lookAtGripper = new LookAtTheGripper("lookAtGripper");
02426          addNode(lookAtGripper);
02427 
02428          VerifyObjectInGripper *verifyObjGrabbed = new VerifyObjectInGripper("verifyObjGrabbed");
02429          addNode(verifyObjGrabbed);
02430 
02431          PostMachineSuccess *postmachinesuccess7 = new PostMachineSuccess("postmachinesuccess7");
02432          addNode(postmachinesuccess7);
02433 
02434          PostMachineFailure *postmachinefailure12 = new PostMachineFailure("postmachinefailure12");
02435          addNode(postmachinefailure12);
02436 
02437          startnode = lookAtGripper;
02438 
02439          lookAtGripper->addTransition(new CompletionTrans("completiontrans42",verifyObjGrabbed));
02440          verifyObjGrabbed->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans37",postmachinesuccess7,StateNode::successSignal));
02441          verifyObjGrabbed->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans38",postmachinefailure12,StateNode::failureSignal));
02442         }
02443       };
02444   
02445       class PrepareForAnotherGrasp : public StateNode {
02446        public:
02447         PrepareForAnotherGrasp(const std::string &nodename="PrepareForAnotherGrasp") : StateNode(nodename), objFutureEgoPos() {}
02448     
02449         class NeedToLookAround : public StateNode {
02450          public:
02451           NeedToLookAround(const std::string &nodename="NeedToLookAround") : StateNode(nodename) {}
02452           virtual void doStart() {
02453             int &objectTagId = getAncestor<GrabActionRunner>()->objectTagId;
02454                             
02455                   std::cout << "[" << getName() << "]: checking if robot needs to look around\n";
02456                   // get a particular april tag
02457                   ShapeRoot tag = find_if(localShS, Kodu::HasAprilTagID(objectTagId));
02458                   postStateSignal<bool>(!tag.isValid());
02459           }
02460         };
02461     
02462         class LookAroundForObject : public MapBuilderNode {
02463          public:
02464           LookAroundForObject(const std::string &nodename="LookAroundForObject") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
02465           virtual void doStart() {
02466                   std::cout << "[" << getName() << "]: attempting to locate object (again)\n";
02467                   // generate points to look at (near the robot's body)
02468                   // these points could also be generated by looking at the gripper's y-pos
02469                   std::vector<Point> pts;
02470                   pts.push_back(Point(  300,    0,    0, egocentric));
02471                   pts.push_back(Point(  500,    0,    0, egocentric));
02472                   pts.push_back(Point(  500, -200,    0, egocentric));
02473                   pts.push_back(Point(  300, -200,    0, egocentric));
02474                   NEW_SHAPE(gazePoints, PolygonData, new PolygonData(localShS, pts, false));
02475                             
02476                   // construct the mapbuilder request
02477                   mapreq.searchArea = gazePoints;
02478                   mapreq.setAprilTagFamily();
02479                   mapreq.clearLocal = true;
02480           }
02481         };
02482     
02483         class RepositionBody : public PilotNode {
02484          public:
02485           RepositionBody(const std::string &nodename="RepositionBody") : PilotNode(nodename,PilotTypes::walk) {}
02486           virtual void doStart() {
02487             Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02488             int &objectTagId = getAncestor<GrabActionRunner>()->objectTagId;
02489             Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
02490                   
02491                   std::cout << "[" << getName() << "]: walking distance = ";
02492                   // get a particular april tag
02493                   ShapeRoot tag = find_if(localShS, Kodu::HasAprilTagID(objectTagId));
02494         
02495                   // get the gripper location
02496                   fmat::Column<3> gripperLoc;
02497         #if defined(TGT_IS_CALLIOPE2)
02498                   gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
02499         #endif
02500                   // where I would really want the objetc to be is the radial reach of the robot
02501                   // plus 50 (or 60) mm
02502                   float objDesiredXPos = gripperLoc[0] + 150.0f;
02503                             
02504                   const Point& kTagPt = tag->getCentroid();
02505                   float objRadius = Kodu::objectRadius(thisAgent->gripperObject);
02506                             
02507                   pilotreq.dx = kTagPt.coordX() - objRadius - objDesiredXPos;
02508                   std::cout << pilotreq.dx << "mm)\n";
02509                             
02510                   // reposition the gripper target object
02511                   objFutureEgoPos = Point(objDesiredXPos, kTagPt.coordY(), (kTagPt.coordZ() / 2.0f),
02512                                           egocentric);
02513                   pilotreq.collisionAction = collisionIgnore;
02514           }
02515         };
02516     
02517         class FindObjectAgain : public MapBuilderNode {
02518          public:
02519           FindObjectAgain(const std::string &nodename="FindObjectAgain") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
02520           virtual void doStart() {
02521             Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02522             Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
02523         
02524                   std::cout << "[" << getName() << "]: looking for gripper object (again)\n";
02525                   // search for the object again using the point calculated previously
02526                   NEW_SHAPE(gazePoint, PointData, new PointData(localShS, objFutureEgoPos));
02527                   mapreq.searchArea = gazePoint;
02528                   const ShapeRoot& targetShape = thisAgent->gripperObject;
02529                   mapreq.addAttributes(targetShape);
02530                   mapreq.maxDist = objFutureEgoPos.xyNorm() + (Kodu::objectRadius(targetShape) * 2.5f);
02531                   mapreq.pursueShapes = true;
02532           }
02533         };
02534     
02535         class RepositionGripperObject : public StateNode {
02536          public:
02537           RepositionGripperObject(const std::string &nodename="RepositionGripperObject") : StateNode(nodename) {}
02538           virtual void doStart() {
02539             Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02540             Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
02541                             
02542                   std::cout << "[" << getName() << "]: attempting to reposition gripper object.\n";
02543                   NEW_SHAPEROOTVEC(objs, subset(localShS,
02544                                                 Kodu::IsShapeOfType(thisAgent->gripperObject->getType())));
02545                   ShapeRoot targetShape = Kodu::getClosestObjectToPoint(objs, objFutureEgoPos);
02546                   // check if the target shape is valid
02547                   if (targetShape.isValid()) {
02548                     std::cout << "target shape is valid. setting its allocentric position.\n";
02549                     Point objAllocentricPt = targetShape->getCentroid();
02550                     objAllocentricPt.applyTransform(VRmixin::mapBuilder->localToWorldMatrix,
02551                                                     allocentric);
02552                     std::cout << "Repositioning the object to from "
02553                               << thisAgent->gripperObject->getCentroid() << " to "
02554                               << objAllocentricPt << std::endl;
02555                     thisAgent->gripperObject->setPosition(objAllocentricPt);
02556                     postStateSuccess();
02557                   }
02558                   // should only fail if the robot did not see any cylinders
02559                   else {
02560                     std::cout << "target shape is NOT VALID!!!! Don't know what to do!\n";
02561                     postStateFailure();
02562                   }
02563           }
02564         };
02565     
02566         virtual void setup() {
02567          NeedToLookAround *needToLook = new NeedToLookAround("needToLook");
02568          addNode(needToLook);
02569 
02570          LookAroundForObject *lookAround = new LookAroundForObject("lookAround");
02571          addNode(lookAround);
02572 
02573          RepositionBody *reposBody = new RepositionBody("reposBody");
02574          addNode(reposBody);
02575 
02576          FindObjectAgain *findObjAgain = new FindObjectAgain("findObjAgain");
02577          addNode(findObjAgain);
02578 
02579          RepositionGripperObject *reposGripObj = new RepositionGripperObject("reposGripObj");
02580          addNode(reposGripObj);
02581 
02582          PostMachineSuccess *postmachinesuccess8 = new PostMachineSuccess("postmachinesuccess8");
02583          addNode(postmachinesuccess8);
02584 
02585          SpeechNode *speechnode10 = new SpeechNode("speechnode10","error shape was not valid");
02586          addNode(speechnode10);
02587 
02588          PostMachineFailure *postmachinefailure13 = new PostMachineFailure("postmachinefailure13");
02589          addNode(postmachinefailure13);
02590 
02591          startnode = needToLook;
02592 
02593          needToLook->addTransition(new SignalTrans<bool>("signaltrans39",lookAround,true));
02594          lookAround->addTransition(new CompletionTrans("completiontrans43",reposBody));
02595          needToLook->addTransition(new SignalTrans<bool>("signaltrans40",reposBody,false));
02596          reposBody->addTransition(new CompletionTrans("completiontrans44",findObjAgain));
02597          findObjAgain->addTransition(new CompletionTrans("completiontrans45",reposGripObj));
02598          reposGripObj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans41",postmachinesuccess8,StateNode::successSignal));
02599          reposGripObj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans42",speechnode10,StateNode::failureSignal));
02600          speechnode10->addTransition(new CompletionTrans("completiontrans46",postmachinefailure13));
02601         }
02602         Point objFutureEgoPos;   // from $provide
02603       };
02604   
02605       class CreateGripMonTask : public StateNode {
02606        public:
02607         CreateGripMonTask(const std::string &nodename="CreateGripMonTask") : StateNode(nodename) {}
02608         virtual void doStart() {
02609           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02610           int &objectTagId = getAncestor<GrabActionRunner>()->objectTagId;
02611               std::cout << "[" << getName() << "]: creating perceptual task; ";
02612               Shape<AprilTagData> tag = find_if<AprilTagData>(localShS,
02613                                                               Kodu::HasAprilTagID(objectTagId));
02614               thisAgent->ptasks.push(
02615                                      new Kodu::VisualGripperMonitorTask(thisAgent->gripperObject, tag));
02616               std::cout << "centroid = " << tag->getCentroid() << "; id = "
02617                         << tag->getTagID();
02618               std::cout << "; angle = " << tag->getCentroid().atanYX() << std::endl;
02619               postStateCompletion();
02620         }
02621       };
02622   
02623       virtual void setup() {
02624        GrabObject *grabObj = new GrabObject("grabObj");
02625        addNode(grabObj);
02626 
02627        VerifyObjectWasGrabbed *verifyObjGrab = new VerifyObjectWasGrabbed("verifyObjGrab");
02628        addNode(verifyObjGrab);
02629 
02630        PrepareForAnotherGrasp *prepForRetry = new PrepareForAnotherGrasp("prepForRetry");
02631        addNode(prepForRetry);
02632 
02633        CreateGripMonTask *createTask = new CreateGripMonTask("createTask");
02634        addNode(createTask);
02635 
02636        PostMachineSuccess *postmachinesuccess9 = new PostMachineSuccess("postmachinesuccess9");
02637        addNode(postmachinesuccess9);
02638 
02639        SpeechNode *speechnode11 = new SpeechNode("speechnode11","unhandled error using grab action");
02640        addNode(speechnode11);
02641 
02642        PostMachineFailure *postmachinefailure14 = new PostMachineFailure("postmachinefailure14");
02643        addNode(postmachinefailure14);
02644 
02645        startnode = grabObj;
02646 
02647        grabObj->addTransition(new GrasperTrans("graspertrans2",verifyObjGrab));
02648        verifyObjGrab->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans43",createTask,StateNode::successSignal));
02649        createTask->addTransition(new CompletionTrans("completiontrans47",postmachinesuccess9));
02650        verifyObjGrab->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans44",prepForRetry,StateNode::failureSignal));
02651        prepForRetry->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans45",grabObj,StateNode::successSignal));
02652        prepForRetry->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans46",speechnode11,StateNode::failureSignal));
02653        speechnode11->addTransition(new CompletionTrans("completiontrans48",postmachinefailure14));
02654       }
02655   
02656     };
02657 
02658     class GrabActionEnd : public StateNode {
02659      public:
02660       GrabActionEnd(const std::string &nodename="GrabActionEnd") : StateNode(nodename) {}
02661       virtual void doStart() {
02662         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02663           std::cout << "[" << getName() << "]: finishing up grab action\n";
02664     
02665           // reposition the gripper object (in the worldShS) outside the world bounds
02666           thisAgent->gripperObject->setPosition(Point(3000.0f, 3000.0f, 0.0f, allocentric));
02667           std::cout << "repositioning the grabbed object to "
02668                     << thisAgent->gripperObject->getCentroid() << std::endl;
02669     
02670           // the agent is not longer attempting to grab the object--the agent succeeded in grabbing it
02671           // the target object is (should be) in the gripper
02672           thisAgent->manipulationComplete();
02673           std::cout << "Grab action complete.\n";
02674           postStateCompletion();
02675       }
02676     };
02677 
02678     virtual void setup() {
02679      GrabActionStart *gStart = new GrabActionStart("gStart");
02680      addNode(gStart);
02681 
02682      PrepareBody *prep = new PrepareBody("prep");
02683      addNode(prep);
02684 
02685      ExecuteGrabAction *exec = new ExecuteGrabAction("exec");
02686      addNode(exec);
02687 
02688      GrabActionEnd *gEnd = new GrabActionEnd("gEnd");
02689      addNode(gEnd);
02690 
02691      SpeechNode *speechnode12 = new SpeechNode("speechnode12","failure during start node in grab action");
02692      addNode(speechnode12);
02693 
02694      PostMachineCompletion *postmachinecompletion5 = new PostMachineCompletion("postmachinecompletion5");
02695      addNode(postmachinecompletion5);
02696 
02697      SpeechNode *speechnode13 = new SpeechNode("speechnode13","failure during execution node in grab action");
02698      addNode(speechnode13);
02699 
02700      startnode = gStart;
02701 
02702      gStart->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans47",prep,StateNode::successSignal));
02703      gStart->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans48",speechnode12,StateNode::failureSignal));
02704      speechnode12->addTransition(new CompletionTrans("completiontrans49",gEnd));
02705      prep->addTransition(new CompletionTrans("completiontrans50",exec));
02706      exec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans49",gEnd,StateNode::successSignal));
02707      gEnd->addTransition(new CompletionTrans("completiontrans51",postmachinecompletion5));
02708      exec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans50",speechnode13,StateNode::failureSignal));
02709      speechnode13->addTransition(new CompletionTrans("completiontrans52",gEnd));
02710     }
02711     int objectTagId;   // from $provide
02712   };
02713 
02714   // end of GrabActionRunner
02715 
02716 
02717   // GiveActionRunner
02718 
02719   class GiveActionRunner : public StateNode {
02720    public:
02721     GiveActionRunner(const std::string &nodename="GiveActionRunner") : StateNode(nodename) {}
02722     class PositionBody : public VisualRoutinesStateNode {
02723      public:
02724       PositionBody(const std::string &nodename="PositionBody") : VisualRoutinesStateNode(nodename), dx() {}
02725   
02726       class Rotate : public WalkNode {
02727        public:
02728         Rotate(const std::string &nodename="Rotate") : WalkNode(nodename) {}
02729         virtual void doStart() {
02730           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02731           float &dx = getAncestor<PositionBody>()->dx;
02732       
02733               const float buffer = 550;
02734               Point target = thisAgent->giveTargetObject->getCentroid() - theAgent->getCentroid();
02735               target = target.unitVector()*(target.xyNorm() - buffer);
02736               req = WalkRequest::displacement(0, 0, target.atanYX());
02737               dx = target.xyNorm();
02738         }
02739       };
02740   
02741       class Walk : public WalkNode {
02742        public:
02743         Walk(const std::string &nodename="Walk") : WalkNode(nodename) {}
02744         virtual void doStart() {
02745           float &dx = getAncestor<PositionBody>()->dx;
02746       
02747               req = WalkRequest::displacement(dx, 0, 0);
02748         }
02749       };
02750   
02751       virtual void setup() {
02752        Rotate *rotate1 = new Rotate("rotate1");
02753        addNode(rotate1);
02754 
02755        Walk *walk1 = new Walk("walk1");
02756        addNode(walk1);
02757 
02758        PostMachineCompletion *postmachinecompletion6 = new PostMachineCompletion("postmachinecompletion6");
02759        addNode(postmachinecompletion6);
02760 
02761        startnode = rotate1;
02762 
02763        rotate1->addTransition(new CompletionTrans("completiontrans53",walk1));
02764        walk1->addTransition(new CompletionTrans("completiontrans54",postmachinecompletion6));
02765       }
02766       float dx;   // from $provide
02767     };
02768 
02769     class Drop : public VisualRoutinesStateNode {
02770      public:
02771       Drop(const std::string &nodename="Drop") : VisualRoutinesStateNode(nodename) {}
02772       virtual void doStart() {
02773         DropActionRunner* &dropActRef = getAncestor<KoduInterpreter>()->dropActRef;
02774           dropActRef->start();
02775           postStateCompletion();
02776       }
02777     };
02778 
02779     class Backup : public PilotNode {
02780      public:
02781       Backup(const std::string &nodename="Backup") : PilotNode(nodename,PilotTypes::walk) {}
02782       virtual void doStart() {
02783           pilotreq = PilotRequest(PilotTypes::walk);
02784           pilotreq.dx = -150;
02785       }
02786     };
02787 
02788     class GiveActionStart : public StateNode {
02789      public:
02790       GiveActionStart(const std::string &nodename="GiveActionStart") : StateNode(nodename) {}
02791               void doStart() {
02792       Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02793                   std::string name = thisAgent->giveTargetObject->getName();
02794                   std::cout << "Giving to " << thisAgent->giveTargetObject->getName() << std::endl;
02795                   bool found = false;
02796                   for (map<int,player_identity>::iterator it = KoduDiscover::players.begin();
02797                        it != KoduDiscover::players.end(); ++it) {
02798                       if (KoduType::strs[it->second.type] == name && it->first != 0) {
02799                           thisAgent->giveTargetObject->setHostAddr(it->first);
02800                           thisAgent->gotGiveReady = false;
02801                           KoduGiveEvent evt;
02802                           evt.setDestination(thisAgent->giveTargetObject->getHostAddr());
02803                           erouter->postEvent(evt);
02804                           erouter->addTimer(this, 0, 200, true);
02805                           found = true;
02806                           std::cout << "Found player at " << it->first << std::endl;
02807                           break;
02808                       }
02809                   }
02810   
02811                   if (!found) {
02812                       std::cout << "Couldn't find player " << name << " in map" << std::endl;
02813                   }
02814               }
02815   
02816         void doEvent() {
02817       Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02818   
02819                   if (event->getGeneratorID() == EventBase::timerEGID) {
02820   
02821                       if (thisAgent->gotGiveReady) {
02822                           erouter->removeTimer(this);
02823                           postStateCompletion();
02824                       }
02825                   }
02826         }
02827     };
02828 
02829     class GiveActionSend : public StateNode {
02830      public:
02831       GiveActionSend(const std::string &nodename="GiveActionSend") : StateNode(nodename) {}
02832       virtual void doStart() {
02833         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02834     
02835           AngTwoPi theirheading = thisAgent->giveTargetObject->getOrientation();
02836           fmat::Column<3> theirpos = thisAgent->giveTargetObject->getCentroid().getCoords();
02837                 fmat::Column<3> gpos;
02838     #if defined(TGT_IS_CALLIOPE2)
02839                 gpos = kine->linkToBase(GripperFrameOffset).translation();
02840     #endif
02841           fmat::Column<3> ourpos = theAgent->getCentroid().getCoords();
02842     
02843                 fmat::Column<3> poffset = ourpos + gpos - theirpos;
02844     
02845           AngSignPi target = atan2(poffset[1], poffset[0]);
02846           AngSignPi offset = target - (AngSignPi)theirheading;
02847     
02848           KoduGiveEvent evt;
02849           evt.setDestination(thisAgent->giveTargetObject->getHostAddr());
02850           evt.setTurnAndObject(offset, thisAgent->gripperObject->getName());
02851           erouter->postEvent(evt);
02852     
02853           thisAgent->manipulationComplete();
02854           thisAgent->setWantsToGiveObjectFlag(false);
02855           thisAgent->setIsExecutingMotionActionFlag(false);
02856           postStateCompletion();
02857       }
02858     };
02859 
02860     virtual void setup() {
02861      GiveActionStart *give = new GiveActionStart("give");
02862      addNode(give);
02863 
02864      GiveActionSend *send = new GiveActionSend("send");
02865      addNode(send);
02866 
02867      PositionBody *positionbody1 = new PositionBody("positionbody1");
02868      addNode(positionbody1);
02869 
02870      Drop *drop1 = new Drop("drop1");
02871      addNode(drop1);
02872 
02873      Backup *backup1 = new Backup("backup1");
02874      addNode(backup1);
02875 
02876      PostMachineCompletion *postmachinecompletion7 = new PostMachineCompletion("postmachinecompletion7");
02877      addNode(postmachinecompletion7);
02878 
02879      startnode = give;
02880 
02881      give->addTransition(new CompletionTrans("completiontrans55",positionbody1));
02882      positionbody1->addTransition(new CompletionTrans("completiontrans56",drop1));
02883      drop1->addTransition(new CompletionTrans("completiontrans57",backup1));
02884      backup1->addTransition(new CompletionTrans("completiontrans58",send));
02885      send->addTransition(new CompletionTrans("completiontrans59",postmachinecompletion7));
02886     }
02887   };
02888 
02889   // end of GiveActionRunner
02890 
02891   // ReceiveActionRunner
02892 
02893   class ReceiveActionRunner : public StateNode {
02894    public:
02895     ReceiveActionRunner(const std::string &nodename="ReceiveActionRunner") : StateNode(nodename) {}
02896 
02897     class ReceiveActionTurnAndGet : public VisualRoutinesStateNode {
02898      public:
02899       ReceiveActionTurnAndGet(const std::string &nodename="ReceiveActionTurnAndGet") : VisualRoutinesStateNode(nodename) {}
02900       class Rotate : public WalkNode {
02901        public:
02902         Rotate(const std::string &nodename="Rotate") : WalkNode(nodename) {}
02903         virtual void doStart() {
02904           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02905               req = WalkRequest::displacement(0, 0, thisAgent->giveAngleToTurn);
02906         }
02907       };
02908   
02909       class LookDown : public HeadPointerNode {
02910        public:
02911         LookDown(const std::string &nodename="LookDown") : HeadPointerNode(nodename) {}
02912         virtual void doStart() {
02913               getMC()->lookAtPoint(400, 0, 0, 0);
02914         }
02915       };
02916   
02917       class FindObject : public MapBuilderNode {
02918        public:
02919         FindObject(const std::string &nodename="FindObject") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
02920         virtual void doStart() {
02921               mapreq.immediateRequest = true;
02922               mapreq.addObjectColor(cylinderDataType, "red");
02923               mapreq.setAprilTagFamily();
02924         }
02925       };
02926   
02927       class Grab : public VisualRoutinesStateNode {
02928        public:
02929         Grab(const std::string &nodename="Grab") : VisualRoutinesStateNode(nodename) {}
02930         virtual void doStart() {
02931           GrabActionRunner* &grabActRef = getAncestor<KoduInterpreter>()->grabActRef;
02932           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
02933               Shape<CylinderData> object;
02934               NEW_SHAPEVEC(apples, CylinderData, select_type<CylinderData>(localShS));
02935               if(apples.size() == 0) {
02936                 NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(localShS));
02937                 if(tags.size() == 0) {
02938                   postStateFailure();
02939                   return;
02940                 }
02941                 Point center = tags[0]->getCentroid();
02942                           for (unsigned int i = 1; i < tags.size(); i++) {
02943                               if (tags[i]->getCentroid().xyNorm() < center.xyNorm()) {
02944                                   center = tags[i]->getCentroid();
02945                               }
02946                           }
02947                 object = Shape<CylinderData>(new CylinderData(localShS, Point(center.coordX(), center.coordY(), 0)));
02948               } else {
02949                           int closest = 0;
02950                           Point center = apples[0]->getCentroid();
02951                           for (unsigned int i = 1; i < apples.size(); i++) {
02952                               if (apples[i]->getCentroid().xyNorm() < center.xyNorm()) {
02953                                   center = apples[i]->getCentroid();
02954                                   closest = i;
02955                               }
02956                           }
02957                 object = apples[closest];
02958               }
02959                       std::cout << "CENTER: " << object->getCentroid() << std::endl;
02960               thisAgent->gripperObject = object;
02961               grabActRef->start();
02962               postStateCompletion();
02963         }
02964       };
02965   
02966       virtual void setup() {
02967        Rotate *rotate2 = new Rotate("rotate2");
02968        addNode(rotate2);
02969 
02970        LookDown *lookdown1 = new LookDown("lookdown1");
02971        addNode(lookdown1);
02972 
02973        StateNode *statenode3 = new StateNode("statenode3");
02974        addNode(statenode3);
02975 
02976        FindObject *findobject1 = new FindObject("findobject1");
02977        addNode(findobject1);
02978 
02979        PostMachineCompletion *postmachinecompletion8 = new PostMachineCompletion("postmachinecompletion8");
02980        addNode(postmachinecompletion8);
02981 
02982        Grab *grab = new Grab("grab");
02983        addNode(grab);
02984 
02985        SpeechNode *speechnode14 = new SpeechNode("speechnode14","failed to see given object");
02986        addNode(speechnode14);
02987 
02988        PostMachineFailure *postmachinefailure15 = new PostMachineFailure("postmachinefailure15");
02989        addNode(postmachinefailure15);
02990 
02991        startnode = rotate2;
02992 
02993        rotate2->addTransition(new CompletionTrans("completiontrans60",lookdown1));
02994        lookdown1->addTransition(new CompletionTrans("completiontrans61",statenode3));
02995        statenode3->addTransition(new TimeOutTrans("timeouttrans2",findobject1,500));
02996        findobject1->addTransition(new CompletionTrans("completiontrans62",grab));
02997        grab->addTransition(new CompletionTrans("completiontrans63",postmachinecompletion8));
02998        grab->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans51",speechnode14,StateNode::failureSignal));
02999        speechnode14->addTransition(new NullTrans("nulltrans1",postmachinefailure15));
03000       }
03001     };
03002 
03003     class ReceiveActionStart : public StateNode {
03004      public:
03005       ReceiveActionStart(const std::string &nodename="ReceiveActionStart") : StateNode(nodename) {}
03006         void doStart() {
03007       Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03008                   std::cout << "Sending give ping-back to " << thisAgent->agentReceivingFrom << std::endl;
03009                   erouter->addTimer(this, 0, 200, true);
03010           KoduGiveEvent evt;
03011           evt.setDestination(thisAgent->agentReceivingFrom);
03012           erouter->postEvent(evt);
03013         }
03014   
03015         void doEvent() {
03016       Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03017                   if (event->getGeneratorID() == EventBase::timerEGID && thisAgent->gotGiveInformation) {
03018                       erouter->removeTimer(this);
03019                       postStateCompletion();
03020                   }
03021         }
03022     };
03023 
03024     class FindObject : public MapBuilderNode {
03025      public:
03026       FindObject(const std::string &nodename="FindObject") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
03027       virtual void doStart() {
03028           std::cout << "[" << getName() << "]: looking for objects.\n";
03029           mapreq.immediateRequest = true;
03030           mapreq.addObjectColor(cylinderDataType, "red");
03031           mapreq.setAprilTagFamily();
03032       }
03033     };
03034 
03035     class ReceiveActionEnd : public StateNode {
03036      public:
03037       ReceiveActionEnd(const std::string &nodename="ReceiveActionEnd") : StateNode(nodename) {}
03038       virtual void doStart() {
03039         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03040           thisAgent->setIsReceiving(false);
03041           postStateCompletion();
03042       }
03043     };
03044 
03045     virtual void setup() {
03046      ReceiveActionStart *recv = new ReceiveActionStart("recv");
03047      addNode(recv);
03048 
03049      ReceiveActionTurnAndGet *turnandget = new ReceiveActionTurnAndGet("turnandget");
03050      addNode(turnandget);
03051 
03052      ReceiveActionEnd *receiveactionend1 = new ReceiveActionEnd("receiveactionend1");
03053      addNode(receiveactionend1);
03054 
03055      PostMachineCompletion *postmachinecompletion9 = new PostMachineCompletion("postmachinecompletion9");
03056      addNode(postmachinecompletion9);
03057 
03058      ReceiveActionEnd *receiveactionend2 = new ReceiveActionEnd("receiveactionend2");
03059      addNode(receiveactionend2);
03060 
03061      PostMachineFailure *postmachinefailure16 = new PostMachineFailure("postmachinefailure16");
03062      addNode(postmachinefailure16);
03063 
03064      startnode = recv;
03065 
03066      recv->addTransition(new CompletionTrans("completiontrans64",turnandget));
03067      turnandget->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans52",receiveactionend1,StateNode::successSignal));
03068      receiveactionend1->addTransition(new CompletionTrans("completiontrans65",postmachinecompletion9));
03069      turnandget->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans53",receiveactionend2,StateNode::failureSignal));
03070      receiveactionend2->addTransition(new CompletionTrans("completiontrans66",postmachinefailure16));
03071     }
03072 
03073   };
03074 
03075   // end of ReceiveActionRunner
03076 
03077 
03078   // MotionActionRunner
03079     
03080   class MotionActionRunner : public StateNode {
03081    public:
03082     MotionActionRunner(const std::string &nodename="MotionActionRunner") : StateNode(nodename), cmd() {}
03083         
03084     virtual void stop() {
03085     Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03086       // set is walking flag to false, and invalidate the current motion command
03087       thisAgent->motionComplete();
03088       cmd = Kodu::MotionCommand();
03089       // abort the pilot
03090       VRmixin::pilot->pilotAbort();
03091       StateNode::stop();
03092     }
03093 
03094     class MotionStart : public StateNode {
03095      public:
03096       MotionStart(const std::string &nodename="MotionStart") : StateNode(nodename) {}
03097       virtual void doStart() {
03098         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03099         Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
03100     
03101           //std::cout << "[" << getName() << "]: starting up motion action runner\n";
03102           //thisAgent->agentIsWalking = true;
03103           thisAgent->signalMotionActionStart();
03104           cmd = thisAgent->currMotionCmd;
03105           postStateCompletion();
03106       }
03107     };
03108 
03109     class ExecuteMotionAction : public StateNode {
03110      public:
03111       ExecuteMotionAction(const std::string &nodename="ExecuteMotionAction") : StateNode(nodename), kMinDistForGoToShape(250.0f), targetShapeTagId(-1) {}
03112   
03113         enum MotionTransType {
03114           MTT_SIMPLE_MOTION = 0,
03115           MTT_GO_TO_SHAPE,
03116           MTT_DRIVER_MOTION
03117         };
03118   
03119       class CheckMotionType : public StateNode {
03120        public:
03121         CheckMotionType(const std::string &nodename="CheckMotionType") : StateNode(nodename) {}
03122         virtual void doStart() {
03123           Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
03124           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03125           Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
03126           int &targetShapeTagId = getAncestor<ExecuteMotionAction>()->targetShapeTagId;
03127           
03128               // *********** temp fix
03129               //std::cout << "[" << getName() << "]: checking motion type\n";
03130           
03131           
03132               // check the type of motion the robot wants to perform
03133               // if the motion command's target shape is valid, then it is a move towards action
03134               
03135               if ( cmd.isDriven() )
03136                 postStateSignal<MotionTransType>(MTT_DRIVER_MOTION);
03137               else if (cmd.targetObjectIsValid()) {
03138                 //std::cout << "looks like a move towards action.\n";
03139           const float &kMinDistForGoToShape = getAncestor<ExecuteMotionAction>()->kMinDistForGoToShape;
03140                 if ( Kodu::distanceInBetweenAgentAndObject(cmd.getTargetObject()) > kMinDistForGoToShape ) {
03141                   //std::cout << "agent is more than " << kMinDistForGoToShape
03142                   //    << "mm away from shape.\n";
03143                   std::cout << "shape was bumped before? ";
03144                   int shapeId = cmd.getTargetObject().getId();
03145                   if (theWorld->shapeTagPairExists(shapeId)) {
03146                     targetShapeTagId = theWorld->getTagIdForShape(shapeId);
03147                     std::cout << "yes! id = " << targetShapeTagId << std::endl;
03148                   } else {
03149                     std::cout << "no.\n";
03150                   }
03151                   postStateSignal<MotionTransType>(MTT_GO_TO_SHAPE);
03152                 } else {
03153                   //std::cout << "agent is too close to execute a goToShape request.\n";
03154                   postStateSignal<MotionTransType>(MTT_SIMPLE_MOTION);
03155                 }
03156                 // *********** temp fix
03157                 // this task should be created anytime the robot is executing a motion action
03158                 // except when doing stationary turns...
03159                 //std::cout << "creating walk progress perceptual task\n";
03160                 thisAgent->ptasks.push(new Kodu::VisualNavErrMonTask(cmd.getTargetObject()));
03161                 // ***********
03162               }
03163               // else it might be something else
03164               else {
03165                 //std::cout << "looks like something that needs simple motion.\n";
03166                 postStateSignal<MotionTransType>(MTT_SIMPLE_MOTION);
03167               }
03168               // ***********
03169         }
03170       };
03171   
03172       class ExecuteGoToShapeRequest : public PilotNode {
03173        public:
03174         ExecuteGoToShapeRequest(const std::string &nodename="ExecuteGoToShapeRequest") : PilotNode(nodename,PilotTypes::goToShape) {}
03175         virtual void doStart() {
03176           Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03177           Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
03178           const float &kMinDistForGoToShape = getAncestor<ExecuteMotionAction>()->kMinDistForGoToShape;
03179               // construct the pilot request
03180               std::cout << "[" << getName() << "]: executing goToShape motion command\n";
03181               pilotreq = PilotRequest(PilotTypes::goToShape);
03182               ShapeRoot target = cmd.getTargetObject();
03183               target->setObstacle(false);
03184               pilotreq.targetShape = target;
03185               // prevent the agent from moving backwards if there is something in the gripper
03186               if (thisAgent->isHoldingAnObject()) {
03187                 //std::cout << "agent is holding an object... disallowing backward motion\n";
03188                 pilotreq.allowBackwardMotion = false;
03189               }
03190               // calculate the offset
03191               float centroidDist = Kodu::distanceFromAgentToObject(target);
03192               float spatialDist = Kodu::distanceInBetweenAgentAndObject(target);
03193               float xOffset = centroidDist - spatialDist + kMinDistForGoToShape;
03194               pilotreq.baseOffset = fmat::pack(xOffset, 0, 0);
03195               cout << "centroidDist=" << centroidDist << " spatialDist=" << spatialDist
03196                    << " baseOffset=" << pilotreq.baseOffset << endl;
03197               // ignore any collisions
03198               pilotreq.collisionAction = collisionIgnore;
03199         }
03200       };
03201   
03202       class ExecuteSimpleMotion : public StateNode {
03203        public:
03204         ExecuteSimpleMotion(const std::string &nodename="ExecuteSimpleMotion") : StateNode(nodename) {}
03205     
03206         class CheckForValidTarget : public StateNode {
03207          public:
03208           CheckForValidTarget(const std::string &nodename="CheckForValidTarget") : StateNode(nodename) {}
03209           virtual void doStart() {
03210             Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
03211                             
03212                   // check if the robot needs to do a final approach
03213                   std::cout << "[" << getName() << "]: robot needs to do final approach? ";
03214                   if (cmd.targetObjectIsValid())
03215                     std::cout << "yes!\n";
03216                   else
03217                     std::cout << "no!\n";
03218                   postStateSignal<bool>(cmd.targetObjectIsValid());
03219           }
03220         };
03221     
03222         class FindMotionTarget : public MapBuilderNode {
03223          public:
03224           FindMotionTarget(const std::string &nodename="FindMotionTarget") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
03225           virtual void doStart() {
03226             Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
03227             int &targetShapeTagId = getAncestor<ExecuteMotionAction>()->targetShapeTagId;
03228                             
03229                   std::cout << "[" << getName() << "]: searching for target object\n";
03230                   // construct the mapbuilder request to search for the target
03231                   mapreq.clearLocal = true;
03232                   const ShapeRoot& kTarget = cmd.getTargetObject();
03233                   // search for a shape with:
03234                   // --- the same color and type
03235                   mapreq.addObjectColor(kTarget->getType(), kTarget->getColor());
03236                   // --- the approx. coordinates
03237                   mapreq.searchArea = kTarget->getCentroidPtShape();
03238                   // --- no centroid beyond this distance
03239                   float centroidDist = Kodu::distanceFromAgentToObject(kTarget);
03240                   float fudgeFactor = Kodu::objectRadius(kTarget) * 1.5f;
03241                   mapreq.maxDist = centroidDist + fudgeFactor;
03242                   // if the target was bumped before look for april tags as well
03243                   if (targetShapeTagId > 0) {
03244                     //std::cout << "searching for april tags too (shape was bumped before).\n";
03245                     mapreq.setAprilTagFamily();
03246                   }
03247                   mapreq.pursueShapes = false;
03248           }
03249         };
03250     
03251         class CreateFinalApproachCommand : public StateNode {
03252          public:
03253           CreateFinalApproachCommand(const std::string &nodename="CreateFinalApproachCommand") : StateNode(nodename) {}
03254           virtual void doStart() {
03255             Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03256             Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
03257             int &targetShapeTagId = getAncestor<ExecuteMotionAction>()->targetShapeTagId;
03258         
03259                   // std::cout << "[" << getName() << "]: creating final approach motion command.\n";
03260                   // check if the object was bumped before, and if that tag was seen in the 
03261                   // recently performed mapbuilder request
03262                   ShapeRoot targetShape = find_if(localShS, Kodu::HasAprilTagID(targetShapeTagId));
03263                   // if the shaperoot reference is valid, then the tag was seen
03264                   if (targetShape.isValid()) {
03265                     //std::cout << "this object was bumped before! using the april tag as target\n";
03266                   }
03267                   // else, the object was either not bumped before or the tag was not seen
03268                   else {
03269                     //  std::cout << "either the tag was not seen, or the object was never bumped; ";
03270                     const ShapeRoot& kTarget = cmd.getTargetObject();
03271                     ShapeRoot lclTarget = VRmixin::mapBuilder->importWorldToLocal(kTarget);
03272                     targetShape = find_if(localShS, Kodu::IsMatchForTargetObject(lclTarget));
03273                     if (!targetShape.isValid()) {
03274                       std::cout << "could not find a match! THIS IS A SERIOUS ERROR!!!\n";
03275                       postStateFailure();
03276                     }
03277                   }
03278                   // get the bearing to the tag
03279                   float angleToTurn = Kodu::bearingFromAgentToObject(targetShape);
03280                   // get the distance to travel:
03281                   // --- get the radius of the object
03282                   float shapeRadius = Kodu::objectRadius(cmd.getTargetObject());
03283                   // --- create a fake cylinder in the world (the shape will be enclosed in this
03284                   //     cylinder)
03285                   NEW_SHAPE(fakeCyl, CylinderData, new CylinderData(localShS,
03286                                                                     targetShape->getCentroid(), 0.0f, shapeRadius));
03287                   // --- calculate the distance in between the agent and this enclosed shape
03288                   float distanceToTravel = Kodu::distanceInBetweenAgentAndObject(fakeCyl);
03289                   // ******* temp fix
03290                   if (thisAgent->isHoldingAnObject())
03291                     distanceToTravel += 45.0f;
03292                   // *******
03293                   // create a new motion command
03294                   cmd = Kodu::MotionCommand(distanceToTravel, angleToTurn, 0.0f, 0.0f);
03295                   //std::cout << "angle = " << angleToTurn << "rads (= "
03296                   //    << rad2deg(angleToTurn) << "degs); distance = "
03297                   //    << distanceToTravel << "mm\n";
03298                   // signal process was successful!
03299                   postStateSuccess();
03300           }
03301         };
03302     
03303         class ExecuteSimpleTurn : public PilotNode {
03304          public:
03305           ExecuteSimpleTurn(const std::string &nodename="ExecuteSimpleTurn") : PilotNode(nodename,PilotTypes::walk) {}
03306           virtual void doStart() {
03307             Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
03308                   // construct the pilot request                    $reference MotionActionRunner::cmd;
03309         
03310                   //std::cout << "[" << getName() << "]: executing simple turn command; da = ";
03311                   pilotreq = PilotRequest(PilotTypes::walk);
03312                   pilotreq.da = cmd.getTurningAngle();
03313                   //std::cout << pilotreq.da << "rads (= " << rad2deg(pilotreq.da) << "degs)\n";
03314                   pilotreq.collisionAction = collisionIgnore;
03315           }
03316         };
03317     
03318         class ExecuteSimpleWalk : public PilotNode {
03319          public:
03320           ExecuteSimpleWalk(const std::string &nodename="ExecuteSimpleWalk") : PilotNode(nodename,PilotTypes::walk) {}
03321           virtual void doStart() {
03322             Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
03323                   // construct the pilot request
03324                   //std::cout << "[" << getName() << "]: executing simple forward walk command; dx = ";
03325                   pilotreq = PilotRequest(PilotTypes::walk);
03326                   pilotreq.dx = cmd.getDistanceToTravel();
03327                   //std::cout << pilotreq.dx << "mm\n";
03328                   pilotreq.collisionAction = collisionIgnore;
03329           }
03330         };
03331     
03332         virtual void setup() {
03333          CheckForValidTarget *checkForValidTarget = new CheckForValidTarget("checkForValidTarget");
03334          addNode(checkForValidTarget);
03335 
03336          FindMotionTarget *findMotionTarget = new FindMotionTarget("findMotionTarget");
03337          addNode(findMotionTarget);
03338 
03339          CreateFinalApproachCommand *createFnlApprCmd = new CreateFinalApproachCommand("createFnlApprCmd");
03340          addNode(createFnlApprCmd);
03341 
03342          ExecuteSimpleTurn *execTurn = new ExecuteSimpleTurn("execTurn");
03343          addNode(execTurn);
03344 
03345          ExecuteSimpleWalk *execWalk = new ExecuteSimpleWalk("execWalk");
03346          addNode(execWalk);
03347 
03348          SpeechNode *finalApprFail = new SpeechNode("finalApprFail","error nothing available for final approach");
03349          addNode(finalApprFail);
03350 
03351          PostMachineFailure *postmachinefailure17 = new PostMachineFailure("postmachinefailure17");
03352          addNode(postmachinefailure17);
03353 
03354          PostMachineCompletion *postmachinecompletion10 = new PostMachineCompletion("postmachinecompletion10");
03355          addNode(postmachinecompletion10);
03356 
03357          startnode = checkForValidTarget;
03358 
03359          checkForValidTarget->addTransition(new SignalTrans<bool>("signaltrans54",findMotionTarget,true));
03360          findMotionTarget->addTransition(new CompletionTrans("completiontrans67",createFnlApprCmd));
03361          checkForValidTarget->addTransition(new SignalTrans<bool>("signaltrans55",execTurn,false));
03362          createFnlApprCmd->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans56",execTurn,StateNode::successSignal));
03363          createFnlApprCmd->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans57",finalApprFail,StateNode::failureSignal));
03364          finalApprFail->addTransition(new CompletionTrans("completiontrans68",postmachinefailure17));
03365          execTurn->addTransition(new CompletionTrans("completiontrans69",execWalk));
03366          execWalk->addTransition(new CompletionTrans("completiontrans70",postmachinecompletion10));
03367         }
03368       };
03369   
03370       class ExecuteDriverMotion : public PilotNode {
03371        public:
03372         ExecuteDriverMotion(const std::string &nodename="ExecuteDriverMotion") : PilotNode(nodename,PilotTypes::walk) {}
03373         virtual void doStart() {
03374               pilotreq = PilotRequest(PilotTypes::setVelocity);
03375         }
03376       };
03377   
03378       virtual void setup() {
03379        CheckMotionType *checkMotionType = new CheckMotionType("checkMotionType");
03380        addNode(checkMotionType);
03381 
03382        ExecuteGoToShapeRequest *execGoToShape = new ExecuteGoToShapeRequest("execGoToShape");
03383        addNode(execGoToShape);
03384 
03385        PostMachineSuccess *postmachinesuccess10 = new PostMachineSuccess("postmachinesuccess10");
03386        addNode(postmachinesuccess10);
03387 
03388        ExecuteSimpleMotion *execSimpleMotion = new ExecuteSimpleMotion("execSimpleMotion");
03389        addNode(execSimpleMotion);
03390 
03391        PostMachineCompletion *postmachinecompletion11 = new PostMachineCompletion("postmachinecompletion11");
03392        addNode(postmachinecompletion11);
03393 
03394        SpeechNode *speechnode15 = new SpeechNode("speechnode15","simple motion failed ");
03395        addNode(speechnode15);
03396 
03397        PostMachineFailure *postmachinefailure18 = new PostMachineFailure("postmachinefailure18");
03398        addNode(postmachinefailure18);
03399 
03400        ExecuteDriverMotion *execDriverMotion = new ExecuteDriverMotion("execDriverMotion");
03401        addNode(execDriverMotion);
03402 
03403        PostMachineFailure *postmachinefailure19 = new PostMachineFailure("postmachinefailure19");
03404        addNode(postmachinefailure19);
03405 
03406        PostMachineCompletion *postmachinecompletion12 = new PostMachineCompletion("postmachinecompletion12");
03407        addNode(postmachinecompletion12);
03408 
03409        startnode = checkMotionType;
03410 
03411        checkMotionType->addTransition(new SignalTrans<MotionTransType>("signaltrans58",execGoToShape,MTT_GO_TO_SHAPE));
03412        checkMotionType->addTransition(new SignalTrans<MotionTransType>("signaltrans59",execSimpleMotion,MTT_SIMPLE_MOTION));
03413        checkMotionType->addTransition(new SignalTrans<MotionTransType>("signaltrans60",execDriverMotion,MTT_DRIVER_MOTION));
03414        execGoToShape->addTransition(new PilotTrans("pilottrans7",postmachinesuccess10,PilotTypes::abort));
03415        execGoToShape->addTransition(new PilotTrans("pilottrans8",execSimpleMotion));
03416        execSimpleMotion->addTransition(new CompletionTrans("completiontrans71",postmachinecompletion11));
03417        execSimpleMotion->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans61",speechnode15,StateNode::failureSignal));
03418        speechnode15->addTransition(new CompletionTrans("completiontrans72",postmachinefailure18));
03419        execDriverMotion->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans62",postmachinefailure19,StateNode::failureSignal));
03420        execDriverMotion->addTransition(new CompletionTrans("completiontrans73",postmachinecompletion12));
03421       }
03422       const float kMinDistForGoToShape;   // from $provide
03423       int targetShapeTagId;   // from $provide
03424     };
03425 
03426     class MotionEnd : public StateNode {
03427      public:
03428       MotionEnd(const std::string &nodename="MotionEnd") : StateNode(nodename) {}
03429       virtual void doStart() {
03430         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03431         Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
03432                 
03433           //std::cout << "[" << getName() << "]: Motion Action Runner complete.\n";
03434           if (thisAgent->currMotionCmd.targetObjectIsValid()) {
03435             //std::cout << "making target shape an obstacle (again).\n";
03436             ShapeRoot targetShape = thisAgent->currMotionCmd.getTargetObject();
03437             targetShape->setObstacle();
03438           }
03439           cmd = Kodu::MotionCommand();
03440           thisAgent->motionComplete();
03441           postStateCompletion();
03442       }
03443     };
03444            
03445 
03446     virtual void setup() {
03447      MotionStart *mStart = new MotionStart("mStart");
03448      addNode(mStart);
03449 
03450      ExecuteMotionAction *execMotion = new ExecuteMotionAction("execMotion");
03451      addNode(execMotion);
03452 
03453      MotionEnd *mEnd = new MotionEnd("mEnd");
03454      addNode(mEnd);
03455 
03456      SpeechNode *speechnode16 = new SpeechNode("speechnode16","error during motion");
03457      addNode(speechnode16);
03458 
03459      PostMachineCompletion *postmachinecompletion13 = new PostMachineCompletion("postmachinecompletion13");
03460      addNode(postmachinecompletion13);
03461 
03462      startnode = mStart;
03463 
03464      mStart->addTransition(new CompletionTrans("completiontrans74",execMotion));
03465      execMotion->addTransition(new CompletionTrans("completiontrans75",mEnd));
03466      execMotion->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans63",speechnode16,StateNode::failureSignal));
03467      speechnode16->addTransition(new CompletionTrans("completiontrans76",mEnd));
03468      mEnd->addTransition(new CompletionTrans("completiontrans77",postmachinecompletion13));
03469     }
03470     Kodu::MotionCommand cmd;   // from $provide
03471   };
03472 
03473   // end of MotionActionRunner
03474 
03475   // PageSwitchActionRunner
03476 
03477   class PageSwitchActionRunner : public StateNode {
03478    public:
03479     PageSwitchActionRunner(const std::string &nodename="PageSwitchActionRunner") : StateNode(nodename), waitCount(0) {}
03480 
03481     class PageSwitchStart : public StateNode {
03482      public:
03483       PageSwitchStart(const std::string &nodename="PageSwitchStart") : StateNode(nodename) {}
03484       virtual void doStart() {
03485           //std::cout << "[" << getName() << "]\n";
03486                 
03487         PerceptualMultiplexor* &multiplexorRef = getAncestor<KoduInterpreter>()->multiplexorRef;
03488         MotionActionRunner* &motionActRef = getAncestor<KoduInterpreter>()->motionActRef;
03489         KoduConditionEvaluator* &evaluatorRef = getAncestor<KoduInterpreter>()->evaluatorRef;
03490         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03491         bool &needToHaltExecution = getAncestor<KoduInterpreter>()->needToHaltExecution;
03492     
03493           //PRINT_ATTRS("Multiplexor is active?", multiplexorRef->isActive());
03494           //PRINT_ATTRS("Multiplexor is in recovery?", multiplexorRef->isInRecovery());
03495           //PRINT_ATTRS("Evaluator is active?", evaluatorRef->isActive());
03496           //PRINT_ATTRS("Agent is walking?", VRmixin::isWalkingFlag);
03497           //PRINT_ATTRS("Attempting manipulation?", thisAgent->isExecutingManipAction());
03498     
03499           if (multiplexorRef->isActive() && !multiplexorRef->isInRecovery()) {
03500             //std::cout << "stopping multiplexor (it is active, but not in recovery mode).\n";
03501             multiplexorRef->stop();
03502           }
03503     
03504           if (needToHaltExecution == false) {
03505             //std::cout << "requesting evaluator to stop... (needToHaltExecution = true)\n";
03506             needToHaltExecution = true;
03507           }
03508     
03509           if (evaluatorRef->isActive()) {
03510             //std::cout << "stopping evaluator.\n";
03511             evaluatorRef->stop();
03512           }
03513     
03514           if (thisAgent->isExecutingMotionAction()) {
03515             //std::cout << "aborting the current motion command.\n";
03516             motionActRef->stop();
03517           }
03518     
03519           if (!evaluatorRef->isActive() && !multiplexorRef->isActive()
03520               && VRmixin::isWalkingFlag == false && !thisAgent->isExecutingManipAction())
03521             {
03522         unsigned int &waitCount = getAncestor<PageSwitchActionRunner>()->waitCount;
03523               if ((++waitCount) == 2) {
03524                 waitCount = 0;
03525                 //std::cout << "proceeding...\n";
03526                 postStateCompletion();
03527                 return;
03528               }
03529             }
03530           //std::cout << "waiting...\n";
03531       }
03532     };
03533 
03534     class SwitchToNewPage : public StateNode {
03535      public:
03536       SwitchToNewPage(const std::string &nodename="SwitchToNewPage") : StateNode(nodename) {}
03537       virtual void doStart() {
03538           std::cout << "[" << getName() << "]: executing the page switch action.\n";
03539         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03540           int newPageIndex = thisAgent->newReqdPage - 1;          
03541           if (thisAgent->pages[newPageIndex] == NULL) {
03542             std::cerr << "!!! A NULL page (index #" << newPageIndex << ") was requested.\n"
03543                       << "Ignoring action.\n";
03544             postStateFailure();
03545             return;
03546           }
03547           // switch to the new page
03548           std::cout << "Switching to page " << thisAgent->newReqdPage
03549                     << " (index #" << newPageIndex << ")\n";
03550           Kodu::KoduPage* nextPage = thisAgent->pages[newPageIndex];
03551     
03552           // reinitialize all the primitives on the new page
03553           const int kRuleCount = nextPage->getRuleCount();
03554           for (int i = 0; i < kRuleCount; i++) {
03555             nextPage->getRuleInPos(i)->reinitializePrimitives();
03556           }
03557           thisAgent->currPageIndex = newPageIndex;
03558           nextPage = NULL;
03559       
03560           // if the ptasks queue is not empty, remove all tasks except gripper monitor and localization
03561           if (!thisAgent->ptasks.empty()) {
03562             std::size_t numbOfTasks = thisAgent->ptasks.size();
03563             //std::cout << "ptasks queue has " << numbOfTasks << " task(s)...\n";
03564             Kodu::PerceptualTaskType_t type;
03565             while (numbOfTasks > 0) {
03566               type = thisAgent->ptasks.front()->getType();
03567               if (type == Kodu::PT_GRIPPER_VIS_MONITOR || type == Kodu::PT_VIS_LOCALIZATION) {
03568                 std::cout << "\tadding task #" << thisAgent->ptasks.front()->getTaskId()
03569                           << " to back of queue.\n";
03570                 thisAgent->ptasks.push(thisAgent->ptasks.front());
03571               } else {
03572                 std::cout << "\tdeleting task #" << thisAgent->ptasks.front()->getTaskId()
03573                           << " from the queue.\n";
03574                 GeneralFncs::deletePtr(thisAgent->ptasks.front());
03575               }
03576               thisAgent->ptasks.pop();
03577               numbOfTasks--;
03578             }
03579           } else {
03580             //std::cout << "the perceptual tasks queue is empty... continuing.\n";
03581           }
03582           thisAgent->newReqdPage = 0;
03583           postStateCompletion();
03584       }
03585     };
03586 
03587     class PageSwitchEnd : public StateNode {
03588      public:
03589       PageSwitchEnd(const std::string &nodename="PageSwitchEnd") : StateNode(nodename) {}
03590       virtual void doStart() {
03591         KoduConditionEvaluator* &evaluatorRef = getAncestor<KoduInterpreter>()->evaluatorRef;
03592         bool &needToHaltExecution = getAncestor<KoduInterpreter>()->needToHaltExecution;
03593                 
03594           //std::cout << "[" << getName() << "]: restarting evaluaStateNode";              
03595              
03596           needToHaltExecution = false;
03597           evaluatorRef->start();
03598     
03599           //std::cout << "Page Switch action completion.\n";
03600           postStateCompletion();
03601       }
03602     };
03603 
03604     virtual void setup() {
03605      PageSwitchStart *psStart = new PageSwitchStart("psStart");
03606      addNode(psStart);
03607 
03608      SwitchToNewPage *execSwitch = new SwitchToNewPage("execSwitch");
03609      addNode(execSwitch);
03610 
03611      PageSwitchEnd *psEnd = new PageSwitchEnd("psEnd");
03612      addNode(psEnd);
03613 
03614      PostMachineFailure *postmachinefailure20 = new PostMachineFailure("postmachinefailure20");
03615      addNode(postmachinefailure20);
03616 
03617      PostMachineCompletion *postmachinecompletion14 = new PostMachineCompletion("postmachinecompletion14");
03618      addNode(postmachinecompletion14);
03619 
03620      startnode = psStart;
03621 
03622      psStart->addTransition(new CompletionTrans("completiontrans78",execSwitch));
03623      psStart->addTransition(new TimeOutTrans("timeouttrans3",psStart,500));
03624      execSwitch->addTransition(new CompletionTrans("completiontrans79",psEnd));
03625      execSwitch->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans64",postmachinefailure20,StateNode::failureSignal));
03626      psEnd->addTransition(new CompletionTrans("completiontrans80",postmachinecompletion14));
03627     }
03628     unsigned int waitCount;   // from $provide
03629   };
03630 
03631   // end of PageSwitchActionRunner
03632 
03633   // PlayActionRunner
03634 
03635   class PlayActionRunner : public StateNode {
03636    public:
03637     PlayActionRunner(const std::string &nodename="PlayActionRunner") : StateNode(nodename) {}
03638 
03639     class PlayStart : public StateNode {
03640      public:
03641       PlayStart(const std::string &nodename="PlayStart") : StateNode(nodename) {}
03642       virtual void doStart() {
03643         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03644                                         std::string wavfile = thisAgent->playQueue.front();
03645                                         thisAgent->playQueue.pop();
03646                                         postStateSignal<std::string>(wavfile);
03647       }
03648     };
03649 
03650     class PlayEnd : public StateNode {
03651      public:
03652       PlayEnd(const std::string &nodename="PlayEnd") : StateNode(nodename) {}
03653       virtual void doStart() {
03654         Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03655                                         if (thisAgent->hasSoundsToPlay()) {
03656                                           postStateCompletion();
03657                                         } else {
03658                                           //std::cout << "All play actions complete.\n";
03659                                           postParentCompletion();
03660                                         }
03661       }
03662     };
03663 
03664     virtual void setup() {
03665      PlayStart *pStart = new PlayStart("pStart");
03666      addNode(pStart);
03667 
03668      PlayEnd *pEnd = new PlayEnd("pEnd");
03669      addNode(pEnd);
03670 
03671      SoundNode *soundnode1 = new SoundNode("soundnode1");
03672      addNode(soundnode1);
03673 
03674      startnode = pStart;
03675 
03676      pStart->addTransition(new SignalTrans<std::string>("signaltrans65",soundnode1));
03677      soundnode1->addTransition(new TimeOutTrans("timeouttrans4",pEnd,50));
03678      pEnd->addTransition(new CompletionTrans("completiontrans81",pStart));
03679     }
03680   };
03681 
03682   class PlayActuator : public StateNode {
03683    public:
03684     PlayActuator(const std::string &nodename="PlayActuator") : StateNode(nodename) {}
03685     virtual void doStart() {
03686       Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03687       if (thisAgent->hasSoundsToPlay()) {
03688         //std::cout << "All Play actions complete.\n";
03689         postStateCompletion();
03690         return;
03691       }
03692       std::string wavfile = thisAgent->playQueue.front();
03693       thisAgent->playQueue.pop();
03694       postStateSignal<std::string>(wavfile);
03695     }
03696   };
03697     
03698   // end of Play Actuator
03699 
03700   // Say Actuator
03701 
03702   class SayActuator : public StateNode {
03703    public:
03704     SayActuator(const std::string &nodename="SayActuator") : StateNode(nodename) {}
03705     virtual void doStart() {
03706       Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03707       postStateSignal<std::string>(thisAgent->stringToSpeak);
03708     }
03709   };
03710 
03711   class CompleteSayActuator : public StateNode {
03712    public:
03713     CompleteSayActuator(const std::string &nodename="CompleteSayActuator") : StateNode(nodename) {}
03714     virtual void doStart() {
03715       Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03716   
03717       //Post event to all other robots
03718       erouter->postEvent(KoduSayEvent(thisAgent->stringToSpeak));
03719   
03720       thisAgent->stringToSpeak.clear();
03721       std::cout << "Say action complete.\n";
03722       postStateCompletion();
03723     }
03724   };
03725 
03726   // end of Speech Actuator
03727 
03728   // Score Actuator
03729 
03730   class ScoreActuator : public StateNode {
03731    public:
03732     ScoreActuator(const std::string &nodename="ScoreActuator") : StateNode(nodename) {}
03733     virtual void doStart() {
03734       Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03735       Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
03736       theWorld->applyGlobalScoreChanges(thisAgent->scoreQueue);
03737       std::cout << "All Score actions complete.\n";
03738       postStateCompletion();
03739     }
03740   };
03741 
03742   class NotificationMonitor : public StateNode {
03743    public:
03744     NotificationMonitor(const std::string &nodename="NotificationMonitor") : StateNode(nodename) {}
03745     virtual void doStart(){
03746       erouter->addListener(this, EventBase::buttonEGID);
03747     }
03748 
03749     virtual void doEvent() {
03750     Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
03751     Kodu::KoduState* &kodustate = getAncestor<KoduInterpreter>()->kodustate;
03752 
03753       if ( event->getGeneratorID() == EventBase::buttonEGID
03754            && IS_GAMEPAD_SID(event->getSourceID()) ) {
03755         kodustate->update(event->getGeneratorID(), 
03756                           event->getSourceID(),
03757                           event->getMagnitude());
03758 
03759         if ( thisAgent->currMotionCmd.isDriven() ) {
03760           float xvel = kodustate->joysticky;
03761           float avel = kodustate->joystickx;
03762           float speed = 125.0;
03763           float angvel = M_PI/8.0;
03764           pilot->changeVelocity(speed*xvel, 0.0, angvel*avel);
03765         }
03766 
03767       }
03768     }
03769      
03770   };
03771 
03772   // end of Score ActuatorStateNode               
03773 
03774     
03775   // ==================================================================================== //
03776     
03777   virtual void setup() {
03778    InitializeAgent *initAgent = new InitializeAgent("initAgent");
03779    addNode(initAgent);
03780 
03781    WalkMonitor *walkMon = new WalkMonitor("walkMon");
03782    addNode(walkMon);
03783 
03784    PerceptualMultiplexor *mplex = new PerceptualMultiplexor("mplex");
03785    addNode(mplex);
03786 
03787    KoduConditionEvaluator *evaluator = new KoduConditionEvaluator("evaluator");
03788    addNode(evaluator);
03789 
03790    KoduActionRunner *runner = new KoduActionRunner("runner");
03791    addNode(runner);
03792 
03793    NotificationMonitor *notifier = new NotificationMonitor("notifier");
03794    addNode(notifier);
03795 
03796    StateNode *endState = new StateNode("endState");
03797    addNode(endState);
03798 
03799    DropActionRunner *dropAct = new DropActionRunner("dropAct");
03800    addNode(dropAct);
03801 
03802    KoduConditionEvaluator *koduconditionevaluator1 = new KoduConditionEvaluator("koduconditionevaluator1");
03803    addNode(koduconditionevaluator1);
03804 
03805    GrabActionRunner *grabAct = new GrabActionRunner("grabAct");
03806    addNode(grabAct);
03807 
03808    GiveActionRunner *giveAct = new GiveActionRunner("giveAct");
03809    addNode(giveAct);
03810 
03811    ReceiveActionRunner *recvAct = new ReceiveActionRunner("recvAct");
03812    addNode(recvAct);
03813 
03814    MotionActionRunner *motionAct = new MotionActionRunner("motionAct");
03815    addNode(motionAct);
03816 
03817    SayActuator *sayAct = new SayActuator("sayAct");
03818    addNode(sayAct);
03819 
03820    CompleteSayActuator *compSayAct = new CompleteSayActuator("compSayAct");
03821    addNode(compSayAct);
03822 
03823    SpeechNode *speechnode17 = new SpeechNode("speechnode17");
03824    addNode(speechnode17);
03825 
03826    PlayActuator *playAct = new PlayActuator("playAct");
03827    addNode(playAct);
03828 
03829    SoundNode *soundnode2 = new SoundNode("soundnode2");
03830    addNode(soundnode2);
03831 
03832    ScoreActuator *scoreAct = new ScoreActuator("scoreAct");
03833    addNode(scoreAct);
03834 
03835    PageSwitchActionRunner *pageSwitchAct = new PageSwitchActionRunner("pageSwitchAct");
03836    addNode(pageSwitchAct);
03837 
03838    SpeechNode *speechnode18 = new SpeechNode("speechnode18","unhandled switch to a null page");
03839    addNode(speechnode18);
03840 
03841    startnode = initAgent;
03842 
03843    CompletionTrans *completiontrans82 = new CompletionTrans("completiontrans82",evaluator);
03844    completiontrans82->addDestination(walkMon);
03845    completiontrans82->addDestination(notifier);
03846    initAgent->addTransition(completiontrans82);
03847 
03848    walkMon->addTransition(new TimeOutTrans("timeouttrans5",walkMon,500));
03849    evaluator->addTransition(new CompletionTrans("completiontrans83",runner));
03850    evaluator->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans66",endState,StateNode::failureSignal));
03851    runner->addTransition(new TimeOutTrans("timeouttrans6",evaluator,150));
03852    mplex->addTransition(new CompletionTrans("completiontrans84",endState));
03853    dropAct->addTransition(new CompletionTrans("completiontrans85",endState));
03854    grabAct->addTransition(new CompletionTrans("completiontrans86",endState));
03855    giveAct->addTransition(new CompletionTrans("completiontrans87",endState));
03856    recvAct->addTransition(new CompletionTrans("completiontrans88",endState));
03857    motionAct->addTransition(new CompletionTrans("completiontrans89",endState));
03858    sayAct->addTransition(new SignalTrans<std::string>("signaltrans67",speechnode17));
03859    speechnode17->addTransition(new CompletionTrans("completiontrans90",compSayAct));
03860    compSayAct->addTransition(new CompletionTrans("completiontrans91",endState));
03861    playAct->addTransition(new SignalTrans<std::string>("signaltrans68",soundnode2));
03862    soundnode2->addTransition(new TimeOutTrans("timeouttrans7",playAct,25));
03863    playAct->addTransition(new CompletionTrans("completiontrans92",endState));
03864    scoreAct->addTransition(new CompletionTrans("completiontrans93",endState));
03865    pageSwitchAct->addTransition(new CompletionTrans("completiontrans94",endState));
03866    pageSwitchAct->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans69",speechnode18,StateNode::failureSignal));
03867    speechnode18->addTransition(new CompletionTrans("completiontrans95",endState));
03868     // 
03869     multiplexorRef = mplex;
03870     evaluatorRef = evaluator;
03871     runnerRef = runner;
03872         
03873     // action runner references
03874     dropActRef = dropAct;
03875     grabActRef = grabAct;
03876     giveActRef = giveAct;
03877     recvActRef = recvAct;
03878     motionActRef = motionAct;
03879 
03880 
03881     pageSwitchActRef = pageSwitchAct;
03882     playActRef = playAct;
03883     sayActRef = sayAct;
03884     scoreActRef = scoreAct;
03885   }
03886     
03887  private:
03888   DISALLOW_COPY_ASSIGN(KoduInterpreter);
03889   Kodu::KoduWorld* theWorld;   // from $provide
03890   Kodu::KoduAgent* thisAgent;   // from $provide
03891   KoduDiscover* discover;   // from $provide
03892   DropActionRunner* dropActRef;   // from $provide
03893   GrabActionRunner* grabActRef;   // from $provide
03894   GiveActionRunner* giveActRef;   // from $provide
03895   ReceiveActionRunner* recvActRef;   // from $provide
03896   MotionActionRunner* motionActRef;   // from $provide
03897   PageSwitchActionRunner* pageSwitchActRef;   // from $provide
03898   PlayActuator* playActRef;   // from $provide
03899   SayActuator* sayActRef;   // from $provide
03900   ScoreActuator* scoreActRef;   // from $provide
03901   PerceptualMultiplexor* multiplexorRef;   // from $provide
03902   bool needToHaltExecution;   // from $provide
03903   KoduConditionEvaluator* evaluatorRef;   // from $provide
03904   KoduActionRunner* runnerRef;   // from $provide
03905   KoduConfig* koduconfig;   // from $provide
03906   KoduEventListener* elistener;   // from $provide
03907   Kodu::KoduState* kodustate;   // from $provide
03908 };

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:43 2016 by Doxygen 1.6.3