Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Pilot.cc

Go to the documentation of this file.
00001 #include "Behaviors/Transitions/CompletionTrans.h"
00002 #include "Behaviors/Transitions/EventTrans.h"
00003 #include "Behaviors/Transitions/TimeOutTrans.h"
00004 #include "Events/EventRouter.h"
00005 #include "Events/LocomotionEvent.h"
00006 
00007 #include "MapBuilder.h"
00008 #include "Lookout.h"
00009 #include "VRmixin.h"
00010 
00011 #include "ShapeTarget.h"
00012 
00013 #include "Pilot.h"
00014 
00015 using namespace std;
00016 
00017 namespace DualCoding {
00018 
00019 Pilot::Pilot() : 
00020   StateNode("Pilot"), requests(), curReq(NULL), idCounter(0), 
00021   waypointwalk_id(MotionManager::invalid_MC_ID), posture_id(MotionManager::invalid_MC_ID),
00022   lastDisplayParticleTime(0),
00023   dispatchNode(NULL)
00024 {
00025   setRetain(false); // don't want VRmixin-based subnodes to persist when Pilot is stopped
00026 }
00027 
00028 
00029 void Pilot::DoStart() {
00030   StateNode::DoStart();
00031   SharedObject<WaypointWalkMC> waypointwalk_mc;
00032   waypointwalk_id = motman->addPersistentMotion(waypointwalk_mc,MotionManager::kIgnoredPriority);
00033   SharedObject<PostureMC> posture_mc;
00034   posture_id = motman->addPersistentMotion(posture_mc,MotionManager::kIgnoredPriority);
00035   if ( verbosity & PVstart ) 
00036     cout << "Pilot starting up: waypointwalk_id=" << waypointwalk_id 
00037    << ", posture_id=" << posture_id << endl;
00038 }
00039 
00040 void Pilot::DoStop() {
00041   if ( verbosity & PVstart )
00042     cout << "Pilot is shutting down." << endl;
00043   abort();
00044   motman->removeMotion(waypointwalk_id);
00045   waypointwalk_id = MotionManager::invalid_MC_ID;
00046   motman->removeMotion(posture_id);
00047   posture_id = MotionManager::invalid_MC_ID;
00048   StateNode::DoStop();
00049 }
00050 
00051 void Pilot::abort() {
00052   if ( dispatchNode )
00053     dispatchNode->DoStop();
00054   curReq = NULL;
00055   while (!requests.empty()) {
00056     delete requests.front();
00057     requests.pop();
00058   }
00059   motman->setPriority(waypointwalk_id,MotionManager::kIgnoredPriority);
00060   motman->setPriority(posture_id,MotionManager::kIgnoredPriority);
00061 }
00062 
00063 void Pilot::setup() {
00064   StateNode::setup();
00065   dispatchNode = new Dispatch;
00066   addNode(dispatchNode);
00067 }
00068 
00069 unsigned int Pilot::executeRequest(const PilotRequest &req) {
00070   requests.push(new PilotRequest(req));
00071   const unsigned int id =  ++idCounter;
00072   requests.back()->requestID = id;
00073   executeRequest();
00074   return id;
00075 }
00076 
00077 void Pilot::executeRequest() {
00078   // check carefully because event from prior request completion could
00079   // have caused user code to already queue and start another request
00080   if ( curReq == NULL & !requests.empty() ) {
00081     curReq = requests.front();
00082     if ( verbosity & PVexecute )
00083       cout << "Pilot executing request " << curReq->requestID << endl;
00084     dispatchNode->DoStart();
00085   }
00086 }
00087 
00088 void Pilot::requestComplete(const bool result) {
00089   if ( curReq == NULL ) {
00090     cout << "Pilot::requestComplete had NULL curReq !!!!!!!!!!!!!!!!" << endl;
00091     return;
00092   }
00093   if ( curReq->trackRequest != NULL )
00094     VRmixin::lookout.stopTrack();
00095   const unsigned int reqID = curReq->requestID;
00096   delete curReq;
00097   curReq = NULL;
00098   requests.pop();
00099   motman->setPriority(waypointwalk_id,MotionManager::kIgnoredPriority);
00100   motman->setPriority(posture_id,MotionManager::kIgnoredPriority);
00101   if ( verbosity & PVcomplete )
00102     cout << "Pilot request " << reqID << " complete." << endl;
00103   erouter->postEvent(EventBase(EventBase::pilotEGID, reqID, EventBase::deactivateETID,0, "Pilot Completion",float(result)));
00104   executeRequest();
00105 }
00106 
00107 //================ Dispatch
00108 
00109 void Pilot::Dispatch::setup() {
00110   VisualRoutinesStateNode::setup();
00111   successNode = new Success;
00112   addNode(successNode);
00113   failureNode = new Failure;
00114   addNode(failureNode);
00115 
00116   //================ Walk
00117 
00118   walkNode = new Walk;
00119   addNode(walkNode);
00120   walkNode->setMC(pilot.waypointwalk_id);
00121   walkNode->addTransition(new CompletionTrans(successNode));
00122 
00123   //================ Visual Search
00124 
00125   visualsearchNode = new VisualSearch;
00126   addNode(visualsearchNode);
00127   VisualSearchHaveMap *visualsearchhavemapNode = new VisualSearchHaveMap;
00128   addNode(visualsearchhavemapNode);
00129   VisualSearchWalk *visualsearchwalkNode = new VisualSearchWalk;
00130   visualsearchwalkNode->setMC(pilot.waypointwalk_id);
00131   addNode(visualsearchwalkNode);
00132 
00133   visualsearchNode->addTransition(new EventTrans(visualsearchhavemapNode, EventBase::mapbuilderEGID,
00134                    &(visualsearchNode->mapbuilder_id), EventBase::deactivateETID));
00135   visualsearchhavemapNode->addTransition(new CompletionTrans(visualsearchwalkNode));
00136   visualsearchwalkNode->addTransition(new CompletionTrans(visualsearchNode));
00137 
00138   //================ GotoShape
00139 
00140   gotoshapeNode = new GotoShape;
00141   gotoshapeNode->setMC(pilot.waypointwalk_id);
00142   addNode(gotoshapeNode);
00143   gotoshapeNode->addTransition(new CompletionTrans(successNode));
00144 
00145   //================ GotoTarget
00146 
00147   gotoTargetNode = new GotoTarget;
00148   gotoTargetNode->setMC(pilot.waypointwalk_id);
00149   addNode(gotoTargetNode);
00150   
00151   CreepToShapeStand *gotoTargetStandNode = new CreepToShapeStand;
00152   gotoTargetStandNode->setMC(pilot.posture_id);
00153   addNode(gotoTargetStandNode);
00154 
00155   CreepToShapeWait *gotoTargetWaitNode = new CreepToShapeWait;
00156   addNode(gotoTargetWaitNode);
00157 
00158   CreepToShapeBuildMap *gotoTargetBuildMapNode = new CreepToShapeBuildMap;
00159   addNode(gotoTargetBuildMapNode);
00160   
00161   gotoTargetNode->buildTargetNode = new BuildTarget;
00162   addNode(gotoTargetNode->buildTargetNode);
00163 
00164   gotoTargetNode->addTransition(new CompletionTrans(gotoTargetStandNode));
00165   gotoTargetStandNode->addTransition(new CompletionTrans(gotoTargetWaitNode));
00166   gotoTargetWaitNode->addTransition(new TimeOutTrans(gotoTargetBuildMapNode,2000));
00167   gotoTargetBuildMapNode->addTransition(new EventTrans(gotoTargetNode->buildTargetNode,
00168                EventBase::mapbuilderEGID,
00169                &CreepToShapeBuildMap::mapreq_id,
00170                EventBase::deactivateETID));
00171   gotoTargetNode->buildTargetNode->addTransition(new CompletionTrans(gotoTargetNode));
00172   
00173   //================ PushTarget
00174   
00175   pushTargetNode = new PushTarget;
00176   pushTargetNode->setMC(pilot.waypointwalk_id);
00177   addNode(pushTargetNode);
00178   
00179   CreepToShapeStand *pushTargetStandNode = new CreepToShapeStand;
00180   pushTargetStandNode->setMC(pilot.posture_id);
00181   addNode(pushTargetStandNode);
00182   
00183   CreepToShapeWait *pushTargetWaitNode = new CreepToShapeWait;
00184   addNode(pushTargetWaitNode);
00185 
00186   CreepToShapeBuildMap *pushTargetBuildMapNode = new CreepToShapeBuildMap;
00187   addNode(pushTargetBuildMapNode);
00188   
00189   pushTargetNode->buildTargetNode = new BuildTarget;
00190   addNode(pushTargetNode->buildTargetNode);
00191 
00192   pushTargetNode->addTransition(new CompletionTrans(pushTargetStandNode));
00193   pushTargetStandNode->addTransition(new CompletionTrans(pushTargetWaitNode));
00194   pushTargetWaitNode->addTransition(new TimeOutTrans(pushTargetBuildMapNode,2000));
00195   pushTargetBuildMapNode->addTransition(new EventTrans(pushTargetNode->buildTargetNode,
00196                EventBase::mapbuilderEGID,
00197                &CreepToShapeBuildMap::mapreq_id,
00198                EventBase::deactivateETID));
00199   pushTargetNode->buildTargetNode->addTransition(new CompletionTrans(pushTargetNode));
00200   
00201   //================ CreepToShape
00202 
00203   creeptoshapeNode = new CreepToShape;
00204   creeptoshapeNode->setMC(pilot.waypointwalk_id);
00205   addNode(creeptoshapeNode);
00206 
00207   CreepToShapeStand *creeptoshapestandNode = new CreepToShapeStand;
00208   creeptoshapestandNode->setMC(pilot.posture_id);
00209   addNode(creeptoshapestandNode);
00210 
00211   CreepToShapeWait *creeptoshapewaitNode = new CreepToShapeWait;
00212   addNode(creeptoshapewaitNode);
00213 
00214   CreepToShapeBuildMap *creeptoshapebuildmapNode = new CreepToShapeBuildMap;
00215   addNode(creeptoshapebuildmapNode);
00216 
00217   CreepToShapeLocalize *creeptoshapelocalizeNode = new CreepToShapeLocalize;
00218   addNode(creeptoshapelocalizeNode);
00219 
00220   creeptoshapeNode->addTransition(new CompletionTrans(creeptoshapestandNode));
00221   creeptoshapestandNode->addTransition(new CompletionTrans(creeptoshapewaitNode));
00222   creeptoshapewaitNode->addTransition(new TimeOutTrans(creeptoshapebuildmapNode,500));
00223   creeptoshapebuildmapNode->addTransition(new EventTrans(creeptoshapelocalizeNode,
00224                EventBase::mapbuilderEGID,
00225                &CreepToShapeBuildMap::mapreq_id,
00226                EventBase::deactivateETID));
00227   creeptoshapelocalizeNode->addTransition(new CompletionTrans(creeptoshapeNode));
00228 
00229   //================ IRCliff and IRObstacle
00230 #ifdef TGT_HAS_IR_DISTANCE
00231 #ifdef TGT_ERS7
00232   ircliffNode = new IRCliff;
00233   addNode(ircliffNode);
00234 #endif //TGT_ERS7
00235   irobstacleNode = new IRObstacle;
00236   addNode(irobstacleNode);
00237 #endif // TGT_HAS_IR_DISTANCE
00238 
00239   //================ Localize
00240 
00241   localizeNode = new Localize;
00242   addNode(localizeNode);
00243 
00244 }
00245 
00246 void Pilot::Dispatch::DoStart() {
00247   VisualRoutinesStateNode::DoStart();
00248 #ifdef TGT_HAS_IR_DISTANCE
00249 #ifdef TGT_ERS7
00250   if ( pilot.curReq->avoidCliffs )
00251     ircliffNode->DoStart();
00252 #endif // TGT_ERS7
00253   if ( pilot.curReq->avoidObstacles )
00254     irobstacleNode->DoStart();
00255 #endif // TGT_HAS_IR_DISTANCE
00256   if ( pilot.curReq->trackRequest )
00257     if ( lookout.busy() )
00258       cout << "Error: Pilot cannot launch track request because the Lookout is busy!" << endl;
00259     else
00260       lookout.executeRequest(*pilot.curReq->trackRequest);
00261   if ( pilot.curReq->localizationInterval > 0 )
00262     localizeNode->DoStart();
00263   else if ( pilot.curReq->localizationDisplayInterval > 0 )
00264     cout << "Pilot warning: localizationDisplayInterval > 0 but no localization requested." << endl;
00265   switch ( pilot.curReq->getRequestType() ) {
00266   case PilotRequest::walk:
00267     walkNode->DoStart();
00268     break;
00269   case PilotRequest::visualSearch:
00270     visualsearchNode->DoStart();
00271     break;
00272   case PilotRequest::gotoShape:
00273     gotoshapeNode->DoStart();
00274     break;
00275   case PilotRequest::gotoTarget:
00276     gotoTargetNode->DoStart();
00277     break;
00278   case PilotRequest::pushTarget:
00279     pushTargetNode->DoStart();
00280     break;
00281   case PilotRequest::creepToShape:
00282     if ( pilot.curReq->mapBuilderRequest == NULL || 
00283    pilot.curReq->mapBuilderRequest->worldTargets.size() == 0 ||
00284    pilot.curReq->trackRequest != NULL ) {
00285       cout << "Pilot error: creeptoShape requires mapBuilderRequest with a worldTarget, and no trackRequest" << endl;
00286       VRmixin::pilot.failure();
00287     }
00288     else
00289       creeptoshapeNode->DoStart();
00290     break;
00291   default:
00292     cout << "Pilot request type " << pilot.curReq->getRequestType() << " not yet implemented." << endl;
00293     VRmixin::pilot.failure();
00294   }
00295 }
00296     
00297 //================ Success and Failure
00298 
00299 void Pilot::Success::DoStart() {
00300   StateNode::DoStart();
00301   if ( Pilot::getVerbosity() & Pilot::PVsuccess )
00302     cout << "Pilot result: Success" << endl;
00303   VRmixin::pilot.dispatchNode->DoStop();
00304   VRmixin::pilot.requestComplete(true);
00305 }
00306 
00307 void Pilot::Failure::DoStart() {
00308   StateNode::DoStart();
00309   if ( Pilot::getVerbosity() & Pilot::PVfailure )
00310     cout << "Pilot result: Failure" << endl;
00311   VRmixin::pilot.dispatchNode->DoStop();
00312   VRmixin::pilot.requestComplete(false);
00313 }
00314 
00315 //================ Walk
00316 
00317 void Pilot::Walk::DoStart() {
00318   WaypointWalkNode::DoStart();
00319   PilotRequest &req = *VRmixin::pilot.curReq;
00320   getMC()->getWaypointList().clear();
00321   getMC()->addEgocentricWaypoint(req.dx/1000, req.dy/1000, req.da, true, 0.1);
00322   getMC()->go();
00323 }
00324 
00325 //================ VisualSearch
00326 
00327 void Pilot::VisualSearch::DoStart() {
00328   VisualRoutinesStateNode::DoStart();
00329   if ( pilot.curReq->mapBuilderRequest == NULL ) {
00330     cout << "Pilot received visualSearch request with invalid mapBuilderRequest field" << endl;
00331     pilot.failure();
00332   }
00333   else if ( pilot.curReq->exitTest == NULL &&  pilot.curReq->searchRotationAngle != 0 ) {
00334     cout << "Pilot received visualSearch request with no exitTest and nonzero searchRotationAngle" << endl;
00335     pilot.failure();
00336   }
00337   else
00338     mapbuilder_id = VRmixin::mapBuilder.executeRequest(*pilot.curReq->mapBuilderRequest);
00339 }
00340 
00341 void Pilot::VisualSearchHaveMap::DoStart() {
00342   VisualRoutinesStateNode::DoStart();
00343   if ( (*pilot.curReq->exitTest)() )
00344     pilot.success();
00345   else if ( pilot.curReq->searchRotationAngle == 0 )
00346     pilot.failure();
00347   else
00348     postCompletionEvent();  // go to VisualSearchWalk to turn body
00349 }
00350 
00351 void Pilot::VisualSearchWalk::DoStart() {
00352   WaypointWalkNode::DoStart();
00353   getMC()->getWaypointList().clear();
00354   getMC()->addEgocentricWaypoint(0,0,VRmixin::pilot.curReq->searchRotationAngle,true,0);
00355   getMC()->go();
00356 }
00357 
00358 //================ GotoShape
00359 
00360 void Pilot::GotoShape::DoStart() {
00361   WaypointWalkNode::DoStart();
00362   const ShapeRoot &target = VRmixin::pilot.curReq->targetShape;
00363   Point const targetLoc(target->getCentroid());
00364   getMC()->getWaypointList().clear();
00365   switch ( target->getRefFrameType() ) {
00366   case unspecified:
00367   case camcentric:
00368     cout << "Pilot: GotoShape target " << target << " has suspect reference frame type; assuming egocentric." << endl;
00369     // fall through to next case
00370   case egocentric: {
00371     AngSignPi bearing = atan2(targetLoc.coordY(),targetLoc.coordX());
00372     float const distance = targetLoc.distanceFrom(Point(0,0));
00373     cout << "Pilot egocentric gotoShape: rotate by " << float(bearing)*180/M_PI << " deg., then forward "
00374    << distance << " mm" << endl;
00375     getMC()->addEgocentricWaypoint(0,0,bearing,true,0);
00376     getMC()->addEgocentricWaypoint(distance/1000,0,0,true,0.1);  // convert distance from mm to meters
00377     break;
00378   }
00379   case allocentric:
00380     cout << "Pilot alloocentric gotoShape: " << targetLoc 
00381    << "    current position: " << VRmixin::theAgent->getCentroid() << endl;
00382     getMC()->addAbsoluteWaypoint(targetLoc.coordX()/1000, targetLoc.coordY()/1000, 0, true, 0.1);
00383   }
00384   getMC()->go();
00385 }
00386 
00387 //================ GotoTarget
00388 
00389 // Assumes the target has been set in the Pilot request
00390 void Pilot::GotoTarget::DoStart() {
00391   WaypointWalkNode::DoStart();
00392 
00393   // Set the parameters for building the target
00394   bool buildFrontLeft = true, buildFrontRight = true, buildBackLeft = true, buildBackRight = true, lookAtCentroid = true;
00395   int maxRetries = 10;
00396   if (VRmixin::pilot.curReq->buildTargetParamsFn != NULL) {
00397     (*VRmixin::pilot.curReq->buildTargetParamsFn)(&buildFrontLeft, &buildFrontRight, &buildBackLeft, &buildBackRight, &lookAtCentroid, &maxRetries);
00398   }
00399   if (buildTargetNode != NULL)
00400     buildTargetNode->setParams(buildFrontLeft, buildFrontRight, buildBackLeft, buildBackRight, lookAtCentroid, maxRetries);
00401 
00402   Point waypoint;
00403   AngSignPi angle;
00404   int result = getNextWaypoint( &waypoint, &angle );
00405   if (result < 0) {
00406     VRmixin::pilot.failure();
00407     return;
00408   }
00409   else if (result > 0) {
00410     VRmixin::pilot.success();
00411     return;
00412   }
00413   
00414   if ( waypoint != Point(0, 0, 0) ) {
00415     if (abs(angle) <= M_PI / 6) {
00416       //cout << "Going to (" << waypoint.coordX() << ", " << waypoint.coordY() << ", " << waypoint.coordZ() << ") and turning " << (float)angle / M_PI * 180.0f << " degrees." << endl;
00417       float currentAngle = getMC()->getCurA() + angle;
00418       getMC()->getWaypointList().clear();
00419       getMC()->addEgocentricWaypoint(waypoint.coordX() / 1000, waypoint.coordY() / 1000, (float)currentAngle, false, 0.1f);
00420       getMC()->go();
00421     }
00422     else {
00423       // Turning angle too large, only turning in place
00424       angle = (angle >= 0) ? M_PI / 6 : -M_PI / 6;
00425       getMC()->getWaypointList().clear();
00426       getMC()->addEgocentricWaypoint(0, 0, (float)angle, true, 0.1f);
00427       getMC()->go();
00428     }
00429   }
00430   else {
00431     //cout << "At destination, only turning " << (float)angle / M_PI * 180.0f << " degrees." << endl;
00432     getMC()->getWaypointList().clear();
00433     getMC()->addEgocentricWaypoint(0, 0, (float)angle, true, 0.1f);
00434     getMC()->go();
00435   }
00436   
00437 }
00438 
00439 /** Calculates the next waypoint and angle to turn in local coordinates.
00440  *  \return 0 on success (got a new waypoint), 1 on success (arrived at destination), -1 on failure
00441  */
00442 int Pilot::GotoTarget::getNextWaypoint(Point *point, AngSignPi *angle) {
00443   const Shape<TargetData>& target = ShapeRootTypeConst( VRmixin::pilot.curReq->targetShape, TargetData );
00444   if (!target.isValid()) {
00445     return -1;
00446   }
00447   if (target->getRefFrameType() != egocentric) {
00448     return -1;
00449   }
00450   
00451   bool pointOK = true;
00452   bool angleOK = false;
00453   
00454   const float orientation = target->getOrientation();
00455   
00456   // These points are relative to the target (in local coordinates) i.e. offset from target's centroid but not orientation
00457   Point desiredPoint( VRmixin::pilot.curReq->positionRelativeToTarget.coordX() * cos(orientation) - VRmixin::pilot.curReq->positionRelativeToTarget.coordY() * sin(orientation),
00458                       VRmixin::pilot.curReq->positionRelativeToTarget.coordX() * sin(orientation) + VRmixin::pilot.curReq->positionRelativeToTarget.coordY() * cos(orientation),
00459                       VRmixin::pilot.curReq->positionRelativeToTarget.coordZ() );
00460   Point presentPoint = target->getCentroid() * -1;
00461   
00462   // If we're within 100mm of the desired point, only need to check angle
00463   *point = Point(0, 0, 0);
00464   if (presentPoint.distanceFrom(desiredPoint) > 100) {
00465     pointOK = false;
00466     
00467     // Calculate present and desired angle around target
00468     AngSignPi desiredAngle = atan2(desiredPoint.coordY(), desiredPoint.coordX());
00469     AngSignPi presentAngle = atan2(presentPoint.coordY(), presentPoint.coordX());
00470     
00471     // Go to the desired point if present angle is close to desired angle
00472     if (abs((double)(presentAngle - desiredAngle)) <= VRmixin::pilot.curReq->approachAngle) {
00473       *point = desiredPoint + target->getCentroid();
00474     }
00475     // Go to the next safe point
00476     else {
00477       // decide whether to go clockwise or anti-clockwise
00478       AngSignPi diff = desiredAngle - presentAngle;
00479       if (abs((double)diff) < VRmixin::pilot.curReq->subtendAngle) {
00480         presentAngle = desiredAngle;
00481       }
00482       else if ((double)diff >= 0) {
00483         presentAngle += VRmixin::pilot.curReq->subtendAngle;
00484       }
00485       else {
00486         presentAngle -= VRmixin::pilot.curReq->subtendAngle;
00487       }
00488       *point = getPointAtAngle(presentAngle);
00489     }
00490     
00491     // Cap distance to a maximum of 200mm
00492     *point = Point( point->coordX(), point->coordY(), 0 );
00493     float distance = point->distanceFrom( Point( 0, 0, 0) );
00494     float scaleFactor = distance < 200 ? 1.0 : 200 / distance;
00495     *point = *point * scaleFactor;
00496   }
00497   
00498   Point delta = target->getCentroid() - *point;
00499   *angle = atan2(delta.coordY(), delta.coordX());
00500   
00501   // If the angle is within 15 degrees and no translation is needed, we're done
00502   if (pointOK && (abs( (float)*angle / M_PI * 180.0f ) <= 15)) {
00503     *angle = 0;
00504     angleOK = true;
00505   }
00506   
00507   return ( pointOK && angleOK ) ? 1 : 0;
00508 }
00509 
00510 /** Returns a point in local coordinates corresponding to an angle around the target at
00511  *  the distance given in the pilot request.
00512  */
00513 Point Pilot::GotoTarget::getPointAtAngle(AngSignPi angle) {
00514   Point point(VRmixin::pilot.curReq->safeDistanceAroundTarget * cos(angle),
00515               VRmixin::pilot.curReq->safeDistanceAroundTarget * sin(angle),
00516               VRmixin::pilot.curReq->positionRelativeToTarget.coordZ());
00517   point += VRmixin::pilot.curReq->targetShape->getCentroid();
00518   return point;
00519 }
00520 
00521 //================ PushTarget
00522 
00523 void Pilot::PushTarget::DoStart() {
00524   WaypointWalkNode::DoStart();
00525   
00526   // Set the parameters for building the target
00527   bool buildFrontLeft = true, buildFrontRight = true, buildBackLeft = true, buildBackRight = true, lookAtCentroid = true;
00528   int maxRetries = 10;
00529   if (VRmixin::pilot.curReq->buildTargetParamsFn != NULL) {
00530     (*VRmixin::pilot.curReq->buildTargetParamsFn)(&buildFrontLeft, &buildFrontRight, &buildBackLeft, &buildBackRight, &lookAtCentroid, &maxRetries);
00531   }
00532   if (buildTargetNode != NULL)
00533     buildTargetNode->setParams(buildFrontLeft, buildFrontRight, buildBackLeft, buildBackRight, lookAtCentroid, maxRetries);
00534 
00535   const Shape<TargetData>& target = ShapeRootTypeConst( VRmixin::pilot.curReq->targetShape, TargetData );
00536   
00537   double orientation = target->getOrientation();
00538   // This point is in local coordinates
00539   Point desiredPoint = Point( VRmixin::pilot.curReq->positionRelativeToTarget.coordX() * cos(orientation) - VRmixin::pilot.curReq->positionRelativeToTarget.coordY() * sin(orientation),
00540                               VRmixin::pilot.curReq->positionRelativeToTarget.coordX() * sin(orientation) + VRmixin::pilot.curReq->positionRelativeToTarget.coordY() * cos(orientation),
00541                               VRmixin::pilot.curReq->positionRelativeToTarget.coordZ() )
00542                      + target->getCentroid();
00543                       
00544   // Create push line
00545   AngSignPi pushAngle = (double)VRmixin::pilot.curReq->angleToPushTarget + orientation;
00546   LineData pushLine(VRmixin::localShS, desiredPoint, pushAngle);
00547   
00548   // Get present position relative to target
00549   Point origin(0, 0, 0);
00550   double totalDisplacement = 200.0;
00551   double xDisplacement = 200.0;
00552   double yDisplacement = pushLine.perpendicularDistanceFrom(origin);
00553   // figure out which side of the line the dog is on and change direction if needed
00554   Point leftPoint(pushLine.getCentroid().coordX(), pushLine.getCentroid().coordY() + 100, pushLine.getCentroid().coordZ());
00555   if (pushLine.pointsOnSameSide( leftPoint, origin )) {
00556     yDisplacement = -yDisplacement;
00557   }
00558   
00559   float scaleFactor = abs(yDisplacement) < totalDisplacement ? 1.0 : totalDisplacement / yDisplacement;
00560   yDisplacement = yDisplacement * scaleFactor;
00561   totalDisplacement -= abs(yDisplacement);
00562   if (totalDisplacement <= 0) {
00563     xDisplacement = 0;
00564   }
00565   else {
00566     scaleFactor = xDisplacement < totalDisplacement ? 1.0 : totalDisplacement / xDisplacement;
00567     xDisplacement = xDisplacement * scaleFactor;
00568   }
00569   
00570   if (abs((float)pushAngle) <= M_PI / 6) {
00571     //cout << "PushTarget: turning by " << (float)pushAngle / M_PI * 180.0f << " deg, y by " << yDisplacement << " mm, x by " << xDisplacement << " mm." << endl;
00572   
00573     float currentAngle = getMC()->getCurA() + pushAngle;
00574     
00575     getMC()->getWaypointList().clear();
00576     getMC()->addEgocentricWaypoint(0, 0, (float)pushAngle, true, 0.1f);
00577     if (abs(yDisplacement) > 10) {
00578       getMC()->addEgocentricWaypoint(0, yDisplacement / 1000, currentAngle, false, 0.1f);
00579     }
00580     if (abs(xDisplacement) > 10) {
00581       getMC()->addEgocentricWaypoint(xDisplacement / 1000, 0, currentAngle, false, 0.1f);
00582     }
00583     getMC()->go();
00584   }
00585   else {
00586     // Turn angle too large, only turning in place
00587     pushAngle = (pushAngle >= 0) ? M_PI / 6 : -M_PI / 6;
00588     getMC()->getWaypointList().clear();
00589     getMC()->addEgocentricWaypoint(0, 0, (float)pushAngle, true, 0.1f);
00590     getMC()->go();
00591   }
00592 }
00593 
00594 //================ BuildTarget
00595 void Pilot::BuildTarget::DoStart() {
00596   VisualRoutinesStateNode::DoStart();
00597 
00598   std::cout << "BuildTarget: DoStart" << std::endl;
00599   
00600   erouter->addListener(this, EventBase::mapbuilderEGID);
00601   
00602   triesLeft = maxRetries;
00603   generateMapBuilderRequest();
00604 }
00605 
00606 void Pilot::BuildTarget::DoStop() {
00607   std::cout << "BuildTarget: DoStop" << std::endl;
00608   
00609   myState = none;
00610   VisualRoutinesStateNode::DoStop();
00611 }
00612 
00613 void Pilot::BuildTarget::processEvent(const EventBase& event) {
00614   switch ( event.getGeneratorID() ) {
00615 
00616   case EventBase::mapbuilderEGID: {
00617     if ((myState == build) && (event.getTypeID() == EventBase::deactivateETID)) {
00618       GET_SHAPE(target, TargetData, localShS);
00619       if (target.isValid()) {
00620         target->printParams();
00621       }
00622       
00623       generateMapBuilderRequest();
00624     }
00625     
00626     break;
00627   };
00628   default:
00629     std::cout << "BuildTarget: Unexpected event: " << event.getDescription() << std::endl;
00630   }
00631 }
00632 
00633 void Pilot::BuildTarget::setParams(bool _buildFrontLeft, bool _buildFrontRight, bool _buildBackLeft, bool _buildBackRight, bool _lookAtCentroid, int _maxRetries) {
00634   buildFrontLeft  = _buildFrontLeft;
00635   buildFrontRight = _buildFrontRight;
00636   buildBackLeft   = _buildBackLeft;
00637   buildBackRight  = _buildBackRight;
00638   lookAtCentroid  = _lookAtCentroid;
00639   maxRetries      = _maxRetries;
00640 }
00641 
00642 void Pilot::BuildTarget::generateMapBuilderRequest() {
00643   keepBestTarget();
00644   GET_SHAPE(target, TargetData, localShS);
00645   if (target.isValid()) {
00646     
00647     Point nextPt;
00648     if (getNextPoint(&nextPt)) {
00649       VRmixin::pilot.curReq->targetShape = target;
00650     
00651       //cout << "BuildTarget: no more points to build." << endl;
00652       myState = complete;
00653       postCompletionEvent();
00654       return;
00655     }
00656     
00657     myState = build;
00658     
00659     if ( VRmixin::pilot.curReq->buildTargetMapBuilderRequestFn != NULL ) {
00660       MapBuilderRequest* mapreq = (*VRmixin::pilot.curReq->buildTargetMapBuilderRequestFn)(nextPt);
00661       VRmixin::mapBuilder.executeRequest(*mapreq);
00662       delete mapreq;
00663     }
00664     else {
00665       cout << "BuildTarget: No MapBuilderRequest function provided." << endl;
00666       myState = error;
00667       VRmixin::pilot.failure();
00668       return;
00669     }
00670   }
00671   else {
00672     cout << "BuildTarget: Cannot find target in build phase." << endl;
00673     myState = error;
00674     VRmixin::pilot.failure();
00675     return;
00676   }
00677 }
00678 
00679 void Pilot::BuildTarget::keepBestTarget() {
00680   Shape<TargetData> best;
00681   int bestCount = 0;
00682   std::vector<Shape<TargetData> >target_vector = select_type<TargetData>(localShS);
00683   std::vector<Shape<TargetData> >::iterator targetIterator;
00684   
00685   // Iterates through all targets in local space, and keeps the one with the most number of valid endpoints, or the longest, in that order
00686   for (targetIterator = target_vector.begin(); targetIterator != target_vector.end(); targetIterator++) {
00687     Shape<TargetData> target = *(targetIterator);
00688     int targetCount = 0;
00689     if (target->getFrontLeftPt().isValid())
00690       targetCount++;
00691     if (target->getFrontRightPt().isValid())
00692       targetCount++;
00693     if (target->getBackLeftPt().isValid())
00694       targetCount++;
00695     if (target->getBackRightPt().isValid())
00696       targetCount++;
00697     if (!best.isValid() || (targetCount > bestCount) || ((targetCount == bestCount) && (target->getLength() > best->getLength()))) {
00698       best = target;
00699       bestCount = targetCount;
00700     }
00701   }
00702   if (best.isValid()) {
00703     best->setName("target");
00704     for (targetIterator = target_vector.begin(); targetIterator != target_vector.end(); targetIterator++) {
00705       Shape<TargetData> other_target = *(targetIterator);
00706       if (best != other_target) {
00707         localShS.deleteShape(other_target);
00708       }
00709     }
00710   }
00711 }
00712 
00713 bool Pilot::BuildTarget::getNextPoint(Point *nextPoint) {
00714   if (triesLeft <= 0) {
00715     return true;
00716   }
00717   triesLeft--;
00718   
00719   GET_SHAPE(target, TargetData, localShS);
00720   if (target.isValid()) {
00721     EndPoint &frontLeft  = target->getFrontLeftPt();
00722     EndPoint &frontRight = target->getFrontRightPt();
00723     EndPoint &backLeft   = target->getBackLeftPt();
00724     EndPoint &backRight  = target->getBackRightPt();
00725     
00726     Point zero(0, 0, 0);
00727     
00728     if (buildFrontLeft && target->isFrontValid() && !frontLeft.isValid() && ((frontLeftPt == zero) || (frontLeftPt != frontLeft))) {
00729       frontLeftPt = frontLeft;
00730       cout << "Looking at front left pt: " << frontLeft << endl;
00731       *nextPoint = frontLeft;
00732       return false;
00733     }
00734     else if (buildFrontRight && target->isFrontValid() && !frontRight.isValid() && ((frontRightPt == zero) || (frontRightPt != frontRight))) {
00735       frontRightPt = frontRight;
00736       cout << "Looking at front right pt: " << frontRight << endl;
00737       *nextPoint = frontRight;
00738       return false;
00739     }
00740     else if (buildBackLeft && target->isBackValid() && !backLeft.isValid() && ((backLeftPt == zero) || (backLeftPt != backLeft))) {
00741       backLeftPt = backLeft;
00742       cout << "Looking at back left pt: " << backLeft << endl;
00743       *nextPoint = backLeft;
00744       return false;
00745     }
00746     else if (buildBackRight && target->isBackValid() && !backRight.isValid() && ((backRightPt == zero) || (backRightPt != backRight))) {
00747       backRightPt = backRight;
00748       cout << "Looking at back right pt: " << backRight << endl;
00749       *nextPoint = backRight;
00750       return false;
00751     }
00752     else if (lookAtCentroid) {
00753       lookAtCentroid = false;
00754       if ((buildFrontLeft || buildFrontRight) && ((centroid == zero) || (centroid != target->getFrontCentroid()))) {
00755         centroid = target->getCentroid();
00756         cout << "Looking at centroid: " << centroid << endl;
00757         *nextPoint = centroid;
00758         return false;
00759       }
00760       else if (!buildFrontLeft && !buildFrontRight && (buildBackLeft || buildBackRight) && ((centroid == zero) || (centroid != target->getBackCentroid()))) {
00761         centroid = target->getBackCentroid();
00762         cout << "Looking at back centroid: " << centroid << endl;
00763         *nextPoint = centroid;
00764         return false;
00765       }
00766     }
00767   }
00768   return true;
00769 }
00770   
00771 //================ CreepToShape
00772 
00773 void Pilot::CreepToShape::DoStart() {
00774   WaypointWalkNode::DoStart();
00775   cout << "  >>>>> CreepToShape" << endl;
00776   if ( VRmixin::lookout.busy() )
00777     cout << "Error: Pilot cannot launch track request because the Lookout is busy!" << endl;
00778   else
00779     VRmixin::lookout.executeRequest(LookoutTrackRequest(VRmixin::pilot.curReq->mapBuilderRequest->worldTargets.front()->targetShape));
00780   const ShapeRoot &target = VRmixin::pilot.curReq->targetShape;
00781   Point const targetLoc(target->getCentroid());
00782   getMC()->getWaypointList().clear();
00783   switch ( target->getRefFrameType() ) {
00784   case unspecified:
00785   case camcentric:
00786     cout << "Pilot: GotoShape target " << target << " has suspect reference frame type; assuming egocentric." << endl;
00787     // fall through to next case
00788   case egocentric: {
00789     // can't creep to egocentric shape because motion invalidates localShS, so try to get there in one trajectory
00790     float distance = targetLoc.xyNorm();
00791     AngSignPi bearing = atan2(targetLoc.coordY(),targetLoc.coordX());
00792     cout << "Pilot egocentric creepToShape: rotate by " << float(bearing)*(180.0/M_PI) << " deg., then forward "
00793    << distance << " mm" << endl;
00794     if ( distance >= 200 ) {
00795       getMC()->addEgocentricWaypoint(0,0,bearing,true,0);
00796       getMC()->addEgocentricWaypoint(distance/1000,0,0,true,0.06);  // convert distance from mm to meters
00797       VRmixin::mapBuilder.moveAgent(0.0, 0.0, float(bearing));
00798       VRmixin::mapBuilder.moveAgent(distance, 0.0, 0.0);
00799     } else {
00800       getMC()->addEgocentricWaypoint(distance/1000, 0, VRmixin::theAgent->getOrientation(), false, 0.06);  // convert distance from mm to meters
00801       VRmixin::mapBuilder.moveAgent(distance, 0.0, 0.0);
00802     }      
00803     break;
00804   }
00805   case allocentric: // this is the normal case
00806     Point const agentLoc = VRmixin::theAgent->getCentroid();
00807     float const distance = targetLoc.xyDistanceFrom(agentLoc);
00808     if ( distance < 50 ) {
00809       VRmixin::pilot.success();
00810       return;
00811     }
00812     float const scaleFactor = distance < 200 ? 1.0 : 200/distance;
00813     Point const waypointLoc = agentLoc + (targetLoc-agentLoc)*scaleFactor;  // take at most 200 mm steps
00814     cout << "Pilot creepToShape: "
00815    << "current position: " << agentLoc
00816    << "  next waypoint: " << waypointLoc
00817    << "  target loc: " << targetLoc << endl;
00818     if ( distance > 200 )
00819       getMC()->addAbsoluteWaypoint(waypointLoc.coordX()/1000, waypointLoc.coordY()/1000, 0, true, 0.06);
00820     else
00821       getMC()->addAbsoluteWaypoint(waypointLoc.coordX()/1000, waypointLoc.coordY()/1000, VRmixin::theAgent->getOrientation(), false, 0.06);
00822     float const heading = atan2(waypointLoc.coordY(),waypointLoc.coordX());
00823     VRmixin::mapBuilder.setAgent(Point(waypointLoc.coordX(),
00824                waypointLoc.coordY(),
00825                agentLoc.coordZ(),
00826                allocentric),
00827          heading);
00828     break;
00829   }
00830   getMC()->go();
00831 }
00832 
00833 void Pilot::CreepToShapeStand::DoStop() {
00834   PostureNode::DoStop(); // call parent first so we can override its effects
00835   motman->setPriority(VRmixin::pilot.posture_id,MotionManager::kBackgroundPriority);
00836 }
00837 
00838 void Pilot::CreepToShapeBuildMap::DoStart() {
00839   VisualRoutinesStateNode::DoStart();
00840   VRmixin::lookout.stopTrack();
00841   if ( VRmixin::pilot.curReq->mapBuilderRequest )
00842     VRmixin::mapBuilder.executeRequest(*VRmixin::pilot.curReq->mapBuilderRequest, &mapreq_id);
00843   else if ( VRmixin::pilot.curReq->mapBuilderRequestFn ) {
00844     MapBuilderRequest* mapreq = (*VRmixin::pilot.curReq->mapBuilderRequestFn)();
00845     VRmixin::mapBuilder.executeRequest(*mapreq, &mapreq_id);
00846     delete mapreq;
00847   }
00848 }
00849 
00850 void Pilot::CreepToShapeLocalize::DoStart() {
00851   StateNode::DoStart();
00852   cout << "  >>>>> CreepToShape Localize" << endl;
00853   if ( VRmixin::pilot.curReq->localizationTest != NULL &&
00854        (*VRmixin::pilot.curReq->localizationTest)() == false ) {
00855     VRmixin::pilot.failure();
00856     return;
00857   }
00858   VRmixin::particleFilter->setMinAcceptableWeight(-3);
00859   for ( int i=0; i<8; i++ )
00860     VRmixin::particleFilter->update();
00861   VRmixin::particleFilter->setAgent();
00862   VRmixin::particleFilter->displayParticles();
00863   Point const loc = VRmixin::theAgent->getCentroid();
00864   MMAccessor<WaypointWalkMC>(VRmixin::pilot.waypointwalk_id)->
00865     setCurPos(loc.coordX()/1000, loc.coordY()/1000, VRmixin::theAgent->getOrientation());
00866   // cout << "View world map; then send a text message to continue..." << endl;
00867   // erouter->addListener(this,EventBase::textmsgEGID);
00868   postCompletionEvent();
00869 }
00870 
00871 void Pilot::CreepToShapeLocalize::processEvent(const EventBase&) {
00872     postCompletionEvent();
00873 }
00874 
00875 //================ IRCliff
00876 #ifdef TGT_HAS_IR_DISTANCE
00877 #ifdef TGT_ERS7
00878 
00879 void Pilot::IRCliff::DoStart() {
00880   StateNode::DoStart();
00881   erouter->addListener(this,EventBase::sensorEGID, SensorSrcID::UpdatedSID, EventBase::statusETID);
00882 }
00883 
00884 void Pilot::IRCliff::processEvent(const EventBase &) {
00885   if ( state->sensors[ChestIRDistOffset] > VRmixin::pilot.curReq->cliffThreshold ) {
00886     cout << "Pilot: cliff detected!" << endl;
00887     VRmixin::pilot.failure();
00888   }
00889 }
00890 #endif //TGT_ERS7
00891 #endif //TGT_HAS_IR_DISTANCE
00892 
00893 //================ IRObstacle
00894 #ifdef TGT_HAS_IR_DISTANCE
00895 
00896 inline float getIR() {
00897 #ifdef TGT_ERS7 
00898   if ( state->sensors[FarIRDistOffset] > 400 )  // far IR goes 200-1500; near IR goes 50-500
00899     return state->sensors[FarIRDistOffset];
00900   else
00901     return state->sensors[NearIRDistOffset];
00902 #else 
00903   return state->sensors[IRDistOffset];
00904 #endif
00905 }
00906 
00907 void Pilot::IRObstacle::DoStart() {
00908   StateNode::DoStart();
00909   erouter->addListener(this,EventBase::sensorEGID, SensorSrcID::UpdatedSID, EventBase::statusETID);
00910 }
00911 
00912 void Pilot::IRObstacle::processEvent(const EventBase &) {
00913   if ( getIR() < VRmixin::pilot.curReq->obstacleThreshold ) {
00914     cout << "Pilot: obstacle detected!" << endl;
00915     VRmixin::pilot.failure();
00916   }
00917 }
00918 #endif // TGT_HAS_IR_DISTANCE
00919 
00920 
00921 //================ Localize
00922 void Pilot::Localize::DoStart() {
00923   StateNode::DoStart();
00924   erouter->addTimer(this, 1, VRmixin::pilot.curReq->localizationInterval, true);
00925   if ( VRmixin::pilot.curReq->mapBuilderRequest->immediateRequest() == false )  // see comment "For immediate requests..." below
00926     erouter->addListener(this,EventBase::mapbuilderEGID);
00927 }
00928 
00929 /* For immediate requests, MapBuilder will post a completion event before
00930 executeRequest has returned the request id, so processEvent will not
00931 recognize this as a relevant event.  Therefore we will process the map
00932 when executeRequest returns; the completion event will already have been
00933 posted and ignored. */
00934 
00935 void Pilot::Localize::processEvent(const EventBase& event) {
00936   switch ( event.getGeneratorID() ) {
00937   case EventBase::timerEGID:
00938     mapbuilder_id = VRmixin::mapBuilder.executeRequest(*VRmixin::pilot.curReq->mapBuilderRequest);
00939     if ( VRmixin::pilot.curReq->mapBuilderRequest->immediateRequest() )
00940       processMap(event.getTimeStamp());
00941     break;
00942   case EventBase::mapbuilderEGID:
00943     if ( event.getSourceID() == mapbuilder_id && event.getTypeID() == EventBase::deactivateETID )
00944       processMap(event.getTimeStamp());
00945     break;
00946   default:
00947     break;
00948   }
00949 }
00950 
00951 void Pilot::Localize::processMap(unsigned int timestamp) {
00952   if ( VRmixin::pilot.curReq->localizationTest == NULL || (*VRmixin::pilot.curReq->localizationTest)() ) {
00953     VRmixin::particleFilter->update();
00954     VRmixin::particleFilter->setAgent();
00955     Point loc = VRmixin::theAgent->getCentroid();
00956     MMAccessor<WaypointWalkMC>(VRmixin::pilot.waypointwalk_id)->
00957       setCurPos(loc.coordX()/1000, loc.coordY()/1000, VRmixin::theAgent->getOrientation());
00958   int const dispInt = VRmixin::pilot.curReq->localizationDisplayInterval;
00959   if ( dispInt > 0 )
00960     if ( timestamp >= VRmixin::pilot.lastDisplayParticleTime+dispInt ) {
00961       VRmixin::particleFilter->displayParticles(VRmixin::pilot.curReq->localizationDisplayParticles);
00962       VRmixin::pilot.lastDisplayParticleTime = timestamp;
00963     }
00964   }
00965 }
00966 
00967 Pilot::PilotVerbosity_t  Pilot::verbosity = -1U;
00968 
00969 unsigned int Pilot::CreepToShapeBuildMap::mapreq_id = 0;
00970 
00971 } // namespace

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