#include "Behaviors/StateMachine.h" /* * Wesley Myers * 15-491 * * EggSorting.cc.fsm */ $nodeclass LGNavigation : VisualRoutinesStateNode, LGmixin() { $provide Shape targetPoint; $nodeclass BuildMap : MapBuilderNode($,MapBuilderRequest::worldMap) : doStart { NEW_SHAPE(gazePt, PointData, new PointData(worldShS,Point(400,0,-100,egocentric))); gazePt->setObstacle(false); mapreq.maxDist = 3000; mapreq.motionSettleTime = 2000; mapreq.addMinBlobArea("blue", 25); mapreq.addMinBlobArea("green", 25); mapreq.addObjectColor(ellipseDataType, "blue"); mapreq.addObjectColor(ellipseDataType, "green"); cout << "BuildMap end" << endl; } $nodeclass FigureOutTarget : VisualRoutinesStateNode : doStart { $reference LGNavigation::targetPoint; cout << "FigureOutTarget start" << endl; NEW_SHAPEVEC(targets, EllipseData, select_type(worldShS)); NEW_SHAPEVEC(blue_target, EllipseData, subset(targets, IsColor("blue"))); NEW_SHAPEVEC(green_target, EllipseData, subset(targets, IsColor("green"))); if(blue_target.size() > 1 || green_target.size() > 1) { cout << "Too many blue and green targets." << endl; return; } cout << "About make shape" << endl; NEW_SHAPE(targetLine, LineData, LineData(worldShS, blue_target[0]->getCentroid(), green_target[0]->getCentroid())); targetLine->setObstacle(false); cout << "Made the shape" << endl; Point robotDestination = targetLine->getCentroid(); Point targetP(robotDestination.coordX() * 2, robotDestination.coordY() * 2, 0); NEW_SHAPE(targetDest, PointData, PointData(worldShS, targetP)); targetPoint = targetDest; postStateCompletion(); cout << "FigureOutTarget end" << endl; } $nodeclass ForwardToTarget : PilotNode($, PilotTypes::goToShape) : doStart { $reference LGNavigation::targetPoint; cout << "ForwardToTarget start" << endl; targetPoint->setObstacle(false); pilotreq.targetShape = targetPoint; cout << "ForwardToTarget end" << endl; } $nodeclass StandUp : PilotNode($, PilotTypes::walk) : doStart { cout << "StandUp start" << endl; pilotreq.dx = -100; postStateCompletion(); cout << "StandUp end" << endl; } $nodeclass Backup : PilotNode($, PilotTypes::walk) : doStart { $reference LGNavigation::targetPoint; cout << "Backup start" << endl; pilotreq.dx = targetPoint->getCentroid().coordX() * -0.01; pilotreq.dy = targetPoint->getCentroid().coordY() * -0.01; postStateCompletion(); cout << "Backup end" << endl; } $nodeclass DisplayCamera : LGNode : doStart { uploadCameraImage("camera1.jpg"); displayHtmlText("I see:

"); postStateCompletion(); } $nodeclass DisplayCamera2 : LGNode : doStart { uploadCameraImage("camera2.jpg"); displayHtmlText("I saw:

And now I see:

"); postStateCompletion(); } $setupmachine{ launch: StateNode =TM("start")=> DisplayCamera =C=> BuildMap =MAP=> FigureOutTarget =C=> ForwardToTarget =PILOT=> Backup =PILOT=> DisplayCamera2 } } REGISTER_BEHAVIOR(LGNavigation);