Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Pilot.cc

Go to the documentation of this file.
00001 #include "Motion/WalkMC.h"
00002 #ifdef TGT_HAS_WALK
00003 
00004 #include <vector>
00005 
00006 #include "Crew/Pilot.h"
00007 #include "DualCoding/VRmixin.h"
00008 #include "Events/PilotEvent.h"
00009 # include "Planners/Navigation/ShapeSpacePlannerXYTheta.h"
00010 #include "Shared/mathutils.h" // for isnan fix
00011 #include "Vision/VisualOdometry/VisualOdometry.h"
00012 
00013 namespace DualCoding {
00014 
00015 void Pilot::doStart() {
00016   if (verbosity & PVstart)
00017     cout << "Pilot starting up: walk_id= " << walk_id <<"  waypointwalk_id= " << waypointwalk_id << endl;
00018   setAgent(Point(0,0,0,allocentric),0,false,false);
00019   VRmixin::particleFilter->displayParticles(50);
00020   addNode(new RunMotionModel)->start();
00021   addNode(new CollisionChecker)->start();
00022   addNode(new RunOpticalFlow)->start();
00023 }
00024 
00025 void Pilot::doStop() {
00026   pilotAbort();
00027   if (verbosity & PVstart)
00028     cout << "Pilot is shutting down." << endl;
00029   motman->removeMotion(walk_id);
00030   motman->removeMotion(waypointwalk_id);
00031   waypointwalk_id = MotionManager::invalid_MC_ID;
00032   walk_id = MotionManager::invalid_MC_ID;
00033   delete defaultLandmarkExtractor;
00034   defaultLandmarkExtractor = NULL;
00035 
00036   // The following teardown() call is necessary because setup
00037   // hard-wires the MC_ID for all the walk nodes.  If we leave the
00038   // waypoint walk motion command active when Tekkotsu shuts down,
00039   // it crashes and generates a nasty backtrace.  But if we remove
00040   // the motion command, there's no easy way to put it back because
00041   // setup won't be called again unless we do this teardown() call.
00042   teardown();
00043 }
00044 
00045 void Pilot::doEvent() {
00046   if (event->getGeneratorID() == EventBase::timerEGID && event->getSourceID() == 9999)
00047     executeRequest();
00048   else
00049     cout << "Pilot got unexpected event: " << event->getDescription() << endl;
00050 }
00051 
00052 void Pilot::unwindForNextRequest() {
00053   erouter->addTimer(this,9999,1,false); // allow time to unwind event stack before executing next request
00054 }
00055 
00056 void Pilot::setAgent(const Point &loc, AngTwoPi heading, bool quiet, bool updateWaypoint) {
00057   if (updateWaypoint) {
00058     cout << "Updating waypoint walk to " << loc << " hdg=" << heading << endl;
00059     MMAccessor<WaypointWalkMC> wp_acc(waypointwalk_id);
00060     wp_acc->setCurPos(loc.coordX(), loc.coordY(), heading);
00061   }
00062   VRmixin::mapBuilder->setAgent(loc, heading, quiet);
00063   // Synching the estimate (below) is a no-op if setAgent was
00064   // invoked by Pilot::RunMotionModel, but not if the user called
00065   // setAgent directly.
00066   VRmixin::particleFilter->setPosition(loc.coordX(), loc.coordY(), heading);
00067   VRmixin::particleFilter->synchEstimateToAgent();
00068 }
00069 
00070 unsigned int Pilot::executeRequest(BehaviorBase* requestingBehavior, const PilotRequest& req) {
00071   VRmixin::requireCrew("Pilot");
00072   requests.push(new PilotRequest(req));
00073   const unsigned int reqID = ++idCounter;
00074   requests.back()->requestID = reqID;
00075   requests.back()->requestingBehavior = requestingBehavior;
00076   if (curReq == NULL)
00077     unwindForNextRequest();
00078   else if (verbosity & PVqueued)
00079     cout << "Pilot request " << requests.back()->requestID << " queued." << endl;
00080   return reqID;
00081 }
00082 
00083 void Pilot::executeRequest() {
00084   if (curReq == NULL && !requests.empty()) {
00085     curReq = requests.front();
00086     if (verbosity & PVexecute)
00087       cout << "Pilot executing request " << curReq->requestID
00088            << ": " << PilotTypes::RequestTypeNames[curReq->requestType] << endl;
00089     VRmixin::particleFilter->synchEstimateToAgent();
00090     if ( ! curReq->walkParameters.empty() ) {
00091       MMAccessor<WaypointWalkMC>(VRmixin::pilot->getWaypointwalk_id())->WalkMC::loadFile(curReq->walkParameters.c_str());
00092       MMAccessor<WalkMC>(VRmixin::pilot->getWalk_id())->loadFile(curReq->walkParameters.c_str());
00093     }
00094     dispatch_->start();
00095   }
00096 }
00097 
00098 void Pilot::requestComplete(PilotTypes::ErrorType_t errorType) {
00099   if (curReq == NULL) {
00100     cout << "Pilot::requestComplete had NULL curReq !!!!!!!!!!!!!!!!" << endl;
00101     return;
00102   }
00103   const BehaviorBase* requester = curReq->requestingBehavior;
00104   PilotEvent e(EventBase::pilotEGID,
00105          (requester==NULL) ? curReq->requestID : (size_t)requester,
00106          EventBase::statusETID);
00107   e.requestType = curReq->requestType;
00108   e.errorType = errorType;
00109   if (verbosity & PVcomplete)
00110     cout << "Pilot request " << curReq->requestID << " complete: "
00111    << PilotTypes::ErrorTypeNames[errorType] <<  endl;
00112   requestComplete(e);
00113 }
00114 
00115 void Pilot::requestComplete(const PilotEvent &e) {
00116   //if ( curReq->trackRequest != NULL )
00117   //  VRmixin::lookout.stopTrack();
00118   dispatch_->finish();
00119   // now clean up everything before posting the event
00120   delete curReq->landmarkExtractor;
00121   delete curReq->searchObjectExtractor;
00122   delete curReq;
00123   curReq = NULL;
00124   requests.pop();
00125   // post the event and allow time to unwind before starting on next event in the queue
00126   erouter->postEvent(e);
00127   VRmixin::autoRefreshSketchWorld();
00128   unwindForNextRequest();
00129 }
00130 
00131 void Pilot::setWorldBounds(float minX, float width, float minY, float height) {
00132   VRmixin::particleFilter->setWorldBounds(minX, width, minY, height);
00133 }
00134 
00135 void Pilot::setWorldBounds(const Shape<PolygonData> &bounds) {
00136   VRmixin::particleFilter->setWorldBounds(bounds);
00137 }
00138 
00139 void Pilot::randomizeParticles(float widthIncr, float heightIncr) {
00140   computeParticleBounds(widthIncr, heightIncr);
00141   VRmixin::particleFilter->resetFilter();
00142 }
00143 
00144 void Pilot::computeParticleBounds(float widthIncr, float heightIncr) {
00145   GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
00146   if ( worldBounds.isValid() )
00147     setWorldBounds(worldBounds);
00148   else {
00149     float minX=1e20, maxX=-minX, minY=1e20, maxY=-minY;
00150     SHAPEROOTVEC_ITERATE(VRmixin::worldShS, s) {
00151       BoundingBox2D b(s->getBoundingBox());
00152       minX = min(minX, b.min[0]);
00153       minY = min(minY, b.min[1]);
00154       maxX = max(maxX, b.max[0]);
00155       maxY = max(maxY, b.max[1]);
00156     } END_ITERATE;
00157     setWorldBounds(minX-widthIncr/2, maxX-minX+widthIncr,
00158                    minY-heightIncr/2, maxY-minY+heightIncr);
00159   }
00160 }
00161 
00162 void Pilot::setDefaultLandmarks(const std::vector<ShapeRoot> &landmarks) {
00163   defaultLandmarks = landmarks;
00164 }
00165 
00166 void Pilot::setDefaultLandmarkExtractor(const MapBuilderRequest &mapreq) {
00167   defaultLandmarkExtractor = new MapBuilderRequest(mapreq);
00168   defaultLandmarkExtractor->clearLocal = false;
00169 }
00170 
00171 void Pilot::pilotPop() {
00172   if ( curReq != NULL ) {
00173     if ( verbosity & PVpop )
00174       cout << "Pilot popping current request." << endl;
00175     dispatch_->stop();
00176     VRmixin::pilot->requestComplete(PilotTypes::abort);
00177   }
00178 }
00179 
00180 void Pilot::pilotAbort() {
00181   if ( verbosity & PVabort ) {
00182     if ( curReq == NULL )
00183       cout << "Pilot aborting but there is no current request." << endl;
00184     else
00185       cout << "Pilot aborting." << endl;
00186   }
00187   dispatch_->stop();
00188   VRmixin::isWalkingFlag = false;
00189   if ( curReq != NULL )
00190     VRmixin::pilot->requestComplete(abort);
00191   while (!requests.empty()) {
00192     delete requests.front();
00193     requests.pop();
00194   }
00195 }
00196 
00197 void Pilot::setupLandmarkExtractor() {
00198   if ( curReq->landmarkExitTest == NULL )
00199     curReq->landmarkExitTest = &Pilot::defaultLandmarkExitTest;
00200   if ( curReq->landmarkExtractor != NULL )  // user supplied his own, so go with that
00201     return;
00202   else if ( defaultLandmarkExtractor != NULL ) {
00203     curReq->landmarkExtractor = new MapBuilderRequest(*defaultLandmarkExtractor);
00204     return;
00205   }
00206   vector<ShapeRoot> landmarkTemp;
00207   if ( ! curReq->landmarks.empty() )
00208     landmarkTemp = curReq->landmarks;
00209   else if ( ! defaultLandmarks.empty() )
00210     landmarkTemp = defaultLandmarks;
00211   else {
00212     const vector<ShapeRoot> &wshapes = VRmixin::worldShS.allShapes();
00213     for ( vector<ShapeRoot>::const_iterator it = wshapes.begin();
00214     it != wshapes.end(); it++ )
00215       if ( (*it)->isLandmark() )
00216         landmarkTemp.push_back(*it);
00217   }
00218   if ( landmarkTemp.empty() ) {
00219     // the user specified no landmarks, so look in worldShS for possibilities
00220 
00221     NEW_SHAPEVEC(markers, MarkerData, select_type<MarkerData>(VRmixin::worldShS));
00222     landmarkTemp.insert(landmarkTemp.begin(), markers.begin(), markers.end());
00223     NEW_SHAPEVEC(apriltags, AprilTagData, select_type<AprilTagData>(VRmixin::worldShS));
00224     landmarkTemp.insert(landmarkTemp.begin(), apriltags.begin(), apriltags.end());
00225   }
00226   if ( !landmarkTemp.empty() ) {
00227     curReq->landmarks = landmarkTemp;  // save what we found so the navigation planner can use them
00228     curReq->landmarkExtractor = new MapBuilderRequest(MapBuilderRequest::localMap);
00229     curReq->landmarkExtractor->clearVerbosity = -1U;
00230     curReq->landmarkExtractor->setVerbosity = MapBuilder::MBVimportShapes;
00231     for ( vector<ShapeRoot>::const_iterator it = landmarkTemp.begin();
00232     it != landmarkTemp.end(); it++ )
00233       curReq->landmarkExtractor->addAttributes(*it);
00234   }
00235 }
00236 
00237 bool Pilot::defaultLandmarkExitTest() {
00238   // Test to see if we have enough landmarks to localize.  We should
00239   // be more selective than this, but for now, count the localShS shapes and
00240   // subtract any point shapes, and any AprilTags whose tag ID doesn't match a
00241   // declared landmark.  In the future we could extend this by matching color
00242   // for ellipses/lines/cylinders, checking tag family, maybe more.
00243   std::vector<ShapeRoot> &locs = VRmixin::localShS.allShapes();
00244   int n = locs.size();
00245   SHAPEROOTVEC_ITERATE(locs, s) {
00246     if ( s->isType(pointDataType) )
00247       --n;
00248     else if ( s->isType(aprilTagDataType) ) {
00249       n--;
00250       SHAPEROOTVEC_ITERATE(VRmixin::pilot->curReq->landmarks, lm) {
00251         if ( lm->isType(aprilTagDataType) &&
00252              ShapeRootTypeConst(lm,AprilTagData)->getTagID() ==
00253              ShapeRootTypeConst(s,AprilTagData)->getTagID() ) {
00254           n++;
00255           break;
00256         } END_ITERATE;
00257       }
00258     }
00259   } END_ITERATE;
00260   // std::cout << "defaultLandmarkExitTest: locs=" << locs.size()
00261   //      << "  lms=" << VRmixin::pilot->curReq->landmarks.size() << " n=" << n << std::endl;
00262   return ( n >= 2 );
00263 }
00264 
00265 std::vector<ShapeRoot>
00266   Pilot::calculateVisibleLandmarks(const DualCoding::Point &currentLocation,
00267            AngTwoPi currentOrientation, AngTwoPi maxTurn,
00268            const std::vector<DualCoding::ShapeRoot> &possibleLandmarks) {
00269   std::vector<ShapeRoot> result;
00270   for(unsigned int i = 0; i<possibleLandmarks.size(); i++)
00271     if ( isLandmarkViewable(possibleLandmarks[i], currentLocation, currentOrientation, maxTurn) )
00272       result.push_back(possibleLandmarks[i]);
00273   return result;
00274 }
00275 
00276 bool Pilot::isLandmarkViewable(const DualCoding::ShapeRoot &selectedLandmark,
00277              const DualCoding::Point &currentLocation,
00278              AngTwoPi currentOrientation, AngTwoPi maxTurn) {
00279   AngSignPi bearing = (selectedLandmark->getCentroid() - currentLocation).atanYX();
00280 
00281   // is the camera within our possible field of view (in front of us)?
00282   if ( fabs(bearing) > (CameraHorizFOV/2.0+maxTurn) )
00283     return false;
00284 
00285   // If there are no walls, assume all landmarks are visible,
00286   // but this isn't really true for poster-type markers where
00287   // we need to know the viewing angle.  Will fix this someday.
00288   Shape<PolygonData> boundary;   // either worldBounds or wall
00289   GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
00290   if ( worldBounds.isValid() )
00291     boundary = worldBounds;
00292   else {
00293     GET_SHAPE(wall, PolygonData, VRmixin::worldShS);
00294     boundary = wall;
00295   }
00296   if ( ! boundary.isValid() )
00297     return true;
00298   // do we intersect with the world bounds when looking at marker?
00299   LineData lineOfSight(VRmixin::worldShS, currentLocation, selectedLandmark->getCentroid());
00300   if ( boundary->intersectsLine(lineOfSight) )
00301     return false;
00302 
00303   // this landmark is viewable
00304   return true;
00305 }
00306 
00307 void Pilot::CollisionDispatch::doStart() {
00308   switch ( VRmixin::pilot->curReq->collisionAction ) {
00309   case collisionStop:
00310   case collisionReplan:
00311     erouter->addListener(this, EventBase::pilotEGID, (size_t)VRmixin::pilot, EventBase::statusETID);
00312     break;
00313   default:
00314     break;
00315   }
00316 }
00317 
00318 void Pilot::CollisionDispatch::doEvent() {
00319   // If we're listening for Pilot collisions then we want to stop or
00320   // replan, so rebroadcast with this as the source node to trigger
00321   PilotEvent e(*event);
00322   e.setSourceID((size_t)this);
00323   erouter->postEvent(e);
00324 }
00325 
00326 void Pilot::TerminateDueToCollision::doStart() {
00327   PilotEvent e(*event);  // copy and convert from an internal Pilot event to a public one
00328   e.setSourceID((size_t)VRmixin::pilot->curReq->requestingBehavior);
00329   VRmixin::isWalkingFlag = false;
00330   VRmixin::pilot->requestComplete(e);
00331 }
00332 
00333 //================ Background processes ================
00334 
00335 void Pilot::RunMotionModel::doStart() {
00336   erouter->addTimer(this, 1, 250, true);  // do odometry update every 250 msec
00337 }
00338 
00339 void Pilot::RunMotionModel::doEvent() {
00340   if (event->getGeneratorID() == EventBase::timerEGID) {
00341     VRmixin::particleFilter->updateMotion();
00342     LocalizationParticle estimate = VRmixin::particleFilter->getEstimate();
00343     VRmixin::mapBuilder->setAgent(Point(estimate.x, estimate.y, 0, allocentric), estimate.theta, true);
00344   }
00345 }
00346 
00347 void Pilot::RunOpticalFlow::doStart() {
00348   if ( VRmixin::imageOdometry != NULL )
00349     erouter->addTimer(this, 1, VRmixin::imageOdometry->suggestedFrameRate(), true);
00350 }
00351 
00352 void Pilot::RunOpticalFlow::doEvent() {
00353   if (event->getGeneratorID() == EventBase::timerEGID)
00354     VRmixin::imageOdometry->update();
00355 }
00356 
00357 void Pilot::CollisionChecker::doStart() {
00358   enableDetection();
00359 }
00360 
00361 void Pilot::CollisionChecker::enableDetection() {
00362 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00363   // std::cout << "Enabled collision detection." << std::endl;
00364   erouter->addListener(this, EventBase::buttonEGID, BumpLeftButOffset, EventBase::activateETID);
00365   erouter->addListener(this, EventBase::buttonEGID, BumpRightButOffset, EventBase::activateETID);
00366   erouter->addListener(this, EventBase::buttonEGID, OvercurrentLeftWheelOffset, EventBase::activateETID);
00367   erouter->addListener(this, EventBase::buttonEGID, OvercurrentRightWheelOffset, EventBase::activateETID);
00368   erouter->addTimer(this, overcurrentResetTimer, 1500, true); // reset overcurrentCounter frequently
00369 #else
00370   std::cout << "Warning: Pilot has no collision check method defined for this robot type" << std::endl;
00371 #endif
00372 }
00373 
00374 void Pilot::CollisionChecker::disableDetection(unsigned int howlong=-1U) {
00375 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00376   // std::cout << "Disabling collision detection for " << howlong << " ms." << std::endl;
00377   erouter->removeListener(this, EventBase::buttonEGID);
00378 #endif
00379   erouter->addTimer(this, 1, howlong, false);
00380 }
00381 
00382 void Pilot::CollisionChecker::doEvent() {
00383   if ( event->getGeneratorID() == EventBase::timerEGID ) {
00384     if ( event->getSourceID() == collisionEnableTimer )
00385       enableDetection();
00386 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00387     else if ( event->getSourceID() == overcurrentResetTimer )
00388       overcurrentCounter = 0;
00389     return;
00390 #endif
00391   }
00392   // If we get here, event must have been a button press (bump senssor or overcurrent)
00393   if ( VRmixin::pilot->curReq == NULL || 
00394        VRmixin::pilot->curReq->collisionAction == collisionIgnore )
00395     return;
00396 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00397   if ( event->getSourceID() == BumpLeftButOffset ||
00398        event->getSourceID() == BumpRightButOffset )
00399     reportCollision();
00400   else // overcurrent -- could be transient.  Count 'em and see.
00401     if ( ++overcurrentCounter >= 12 ) {
00402       overcurrentCounter = 0;
00403       reportCollision();
00404     }
00405 #endif
00406 }
00407 
00408 void Pilot::CollisionChecker::reportCollision() {
00409   if ( VRmixin::pilot->verbosity & PVcollision )
00410     cout << "Pilot: collision detected!" << endl;
00411   disableDetection(1000); // ignore any further collisions for the next second
00412   PilotEvent e(EventBase::pilotEGID, (size_t)VRmixin::pilot, EventBase::statusETID);
00413   e.errorType = collisionDetected;
00414   // Note: in the future we might want to add info about where on the
00415   // robot's body the collision occurred.
00416   erouter->postEvent(e);
00417 }
00418 
00419 void Pilot::PlanPath::clearGraphics() {
00420   GET_SHAPE(plannedPath, GraphicsData, VRmixin::worldShS);
00421   plannedPath.deleteShape();
00422   GET_SHAPE(plannerTrees, GraphicsData, VRmixin::worldShS);
00423   plannerTrees.deleteShape();
00424 }
00425 
00426 void Pilot::PlanPath::doPlan(NavigationPlan &plan, Point initPoint, AngTwoPi initHead, PilotRequest &req) {
00427   plan.clear();
00428   clearGraphics();
00429   Point targetPoint = req.targetShape->getCentroid();
00430   if ( targetPoint.getRefFrameType() == egocentric ) {
00431     targetPoint.applyTransform(VRmixin::mapBuilder->localToWorldMatrix);
00432     targetPoint.setRefFrameType(allocentric);
00433   }
00434   AngTwoPi targetHeading = req.targetHeading;
00435   fmat::Column<3> baseOffset = req.baseOffset;
00436 
00437   const bool planForHeading = ! std::isnan((float)targetHeading);
00438   std::cout << "Planning path from " << initPoint << " to point: " << targetPoint;
00439   if ( baseOffset != fmat::ZERO3 )
00440     cout << " target offset " << baseOffset;
00441   if ( planForHeading )
00442     cout << " heading " << float(targetHeading)/M_PI*180 << " deg.";
00443   cout << std::endl;
00444 
00445   /* Decide if we should walk backward: do this if the distance is
00446    less than the max allowed, and the heading change if we don't would
00447    be > 100 degrees, and the target heading doesn't require a
00448    reversal.  Walking backward requires planning a forward path from
00449    the target to the initial position, then reversing both the path
00450    points and the direction of travel.
00451 
00452    We can't use the planner's baseOffset feature if we're walking backwards,
00453    so we must apply the baseOffset here instead.
00454    
00455    *** NOTE: walking backward without a target heading isn't supported yet.
00456 
00457   */
00458   Point baseTemp = Point(fmat::rotationZ(targetHeading)*baseOffset, allocentric);
00459   Point straightVec = (targetPoint - baseTemp) - initPoint;
00460   float straightDist = straightVec.xyNorm();
00461   AngTwoPi straightAng = float(straightVec.atanYX());
00462   AngSignPi straightAngDiff = float(straightAng - initHead);
00463   if ( req.allowBackwardMotion && planForHeading &&
00464        straightDist <= req.maxBackwardDistance && 
00465        abs(straightAngDiff) > M_PI/1.8 &&
00466        abs(AngSignPi(targetHeading - initHead)) < M_PI/1.8 ) {
00467     plan.walkBackward = true;
00468     std:: cout << "Walking backward..." << std::endl;
00469   }
00470   
00471   Point gatePoint = targetPoint;
00472   if ( plan.walkBackward ) {
00473     gatePoint = targetPoint - baseTemp;
00474     baseOffset = fmat::ZERO3;
00475     swap(initPoint, gatePoint);
00476     swap(initHead, targetHeading);
00477   }
00478 
00479   // set up and call the XYTheta path planner
00480   GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
00481   ShapeSpacePlannerXYTheta planner(VRmixin::worldShS, worldBounds, req.obstacleInflation);
00482   std::vector<NodeValue_t> *pathResult = new std::vector<NodeValue_t>;
00483   std::vector<NodeType_t> *treeStartResult = req.displayTree ? new std::vector<NodeType_t> : NULL;
00484   std::vector<NodeType_t> *treeEndResult = req.displayTree ? new std::vector<NodeType_t> : NULL;
00485   GenericRRTBase::PlannerResult2D result =
00486     planner.planPath(initPoint, baseOffset, req.gateLength,
00487                      gatePoint, initHead, targetHeading, req.maxRRTIterations,
00488                      pathResult, treeStartResult, treeEndResult);
00489   if ( plan.walkBackward ) {
00490     if ( result.code == GenericRRTBase::SUCCESS ) {
00491       swap(initPoint, gatePoint); // restore original values
00492       swap(initHead, targetHeading);
00493       // Reverse the path.  This is a little tricky because the first
00494       // element must be the starting position (not a movement step)
00495       // and turns must still PRECEDE translations.
00496       NodeValue_t &final = (*pathResult)[pathResult->size()-1];
00497       NodeValue_t &preFinal = (*pathResult)[pathResult->size()-2];
00498       std::vector<NodeValue_t> *revPath = new std::vector<NodeValue_t>;
00499       revPath->reserve(pathResult->size()+1);
00500       revPath->push_back(NodeValue_t(final.x, final.y, final.theta, 0));
00501       revPath->push_back(NodeValue_t(preFinal.x, preFinal.y, final.theta, 0));
00502       for ( int i = pathResult->size()-3; i >= 0; i-- )
00503         revPath->push_back(NodeValue_t((*pathResult)[i].x, (*pathResult)[i].y,
00504                                        (*pathResult)[i+1].theta, -(*pathResult)[i+2].turn));
00505       revPath->push_back(NodeValue_t((*pathResult)[0].x, (*pathResult)[0].y,
00506                                      (*pathResult)[0].theta, -(*pathResult)[1].turn));
00507       delete pathResult;
00508       pathResult = revPath;
00509     } else if ( result.code == GenericRRTBase::START_COLLIDES )
00510       result.code = GenericRRTBase::END_COLLIDES;
00511     else if ( result.code == GenericRRTBase::END_COLLIDES )
00512       result.code = GenericRRTBase::START_COLLIDES;
00513   } else // not walking backward
00514     if ( result.code == GenericRRTBase::SUCCESS && planForHeading ) {
00515       // add final segment from gate point to target point
00516       AngSignPi headingChange = AngSignPi(targetHeading - pathResult->back().theta);
00517       AngSignTwoPi turn;
00518       if ( RRTNodeXYTheta::safeTurn(pathResult->back(), headingChange, turn, planner.getCC()) ) {
00519         fmat::Column<3> offsetTarget = targetPoint.getCoords() - fmat::rotationZ(targetHeading) * baseOffset;
00520         NodeValue_t final(offsetTarget[0],offsetTarget[1],targetHeading, turn);
00521         pathResult->push_back(final);
00522       }
00523       else
00524         result.code = GenericRRTBase::END_COLLIDES;
00525   }
00526 
00527   if ( req.displayTree ) {
00528     NEW_SHAPE(plannerTrees, GraphicsData, new GraphicsData(VRmixin::worldShS));
00529     planner.plotTree(*treeStartResult, plannerTrees, rgb(0,0,0));
00530     planner.plotTree(*treeEndResult, plannerTrees, rgb(0,192,0));
00531     delete treeStartResult;
00532     delete treeEndResult;
00533   }
00534   if ( req.displayObstacles || req.autoDisplayObstacles ) {
00535     if ( result.code == GenericRRTBase::END_COLLIDES ) {
00536       VRmixin::robotObstaclesPt = targetPoint;
00537       VRmixin::robotObstaclesOri = planForHeading ? targetHeading : AngTwoPi(0);
00538       planner.addObstaclesToShapeSpace(VRmixin::worldShS);
00539     } else if ( req.displayObstacles || result.code != GenericRRTBase::SUCCESS ) {
00540       // for START_COLLIDES or MAX_ITER, or if user explicitly asked for obstacle display
00541       VRmixin::robotObstaclesPt = VRmixin::theAgent->getCentroid();
00542       VRmixin::robotObstaclesOri = VRmixin::theAgent->getOrientation();
00543       planner.addObstaclesToShapeSpace(VRmixin::worldShS);
00544     }
00545   }
00546 
00547   if ( result.code != GenericRRTBase::SUCCESS ) {
00548     switch ( result.code ) {
00549     case GenericRRTBase::START_COLLIDES:
00550       std::cout << "Navigation path planning failed: start state " << result.movingObstacle->toString()
00551     << " is in collision with " << result.collidingObstacle->toString() << ".\n";
00552       break;
00553     case GenericRRTBase::END_COLLIDES:
00554       std::cout << "Navigation path planning failed: end state " << result.movingObstacle->toString()
00555     << " is in collision with " << result.collidingObstacle->toString() << ".\n";
00556       break;
00557     case GenericRRTBase::MAX_ITER:
00558       std::cout << "Navigation path planning failed: max iterations " << req.maxRRTIterations << " exceeded.\n";
00559       break;
00560     default: break; // dummy to suppress compiler warning
00561     }
00562     postStateSignal<GenericRRTBase::PlanPathResultCode>(result.code);
00563     return;
00564   }
00565 
00566   // Let user edit the path if desired, then save in the nav plan
00567   if ( req.pathEditor )
00568     (*req.pathEditor)(pathResult, req);
00569   plan.path = *pathResult;
00570   delete pathResult;
00571 
00572   // Print the (possibly edited) path
00573   if ( verbosity & PVshowPath )
00574     showPath(plan.path);
00575   // Display the path in worlShS if requested
00576   if ( req.displayPath ) {
00577     NEW_SHAPE(plannedPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
00578     ShapeSpacePlannerXYTheta::plotPath(plan.path, plannedPath, rgb(0,0,255));
00579   }
00580   
00581   // Now execute the plan, unless requested not to
00582   if ( req.executePath )
00583     postStateCompletion();
00584   else
00585     postStateFailure();
00586 }
00587 
00588 void Pilot::PlanPath::showPath (const vector<RRTNodeXYTheta::NodeValue_t> &path) {
00589   if ( path.empty() )
00590     std::cout << "[Empty navigation path]" << std::endl;
00591   else
00592     for ( size_t i = 0; i < path.size(); i++ ) {
00593       Point pt(path[i].x, path[i].y, 0, allocentric);
00594       cout << "path[" << i << "] = ";
00595       if ( i == 0 )
00596         std::cout << "start at ";
00597       else
00598         std::cout << "turn " << float(path[i].turn)*180/M_PI << " deg., then ";
00599       std::cout << pt << " hdg " << float(AngTwoPi(path[i].theta)) * 180/M_PI << " deg." << std::endl;
00600     }
00601 }
00602 
00603 void Pilot::ConstructNavPlan::doAnalysis(NavigationPlan &plan, Point initPoint, 
00604            AngTwoPi initHead, PilotRequest &req) {
00605   Point currentPosition = initPoint;
00606   plan.steps.clear();
00607 
00608   for (unsigned int i = 1; i<(plan.path.size()); i++) {
00609 
00610     // Calculate vector between this and next point on path
00611     Point delta = Point(plan.path[i].x - plan.path[i-1].x,
00612       plan.path[i].y - plan.path[i-1].y,
00613       0, allocentric);
00614     const float distanceBetween = delta.xyNorm();
00615     const AngSignTwoPi turn = plan.path[i].turn;
00616     //std::cout << "### delta=" << delta << "  orientation=" << orientation
00617     //        << "  headingChange=" << headingChange;
00618     
00619     // If this step involves a non-negligible heading change, turn  first
00620     //if( fabs(turn) > M_PI/18 )
00621     //  plan.addNavigationStep(turnStep, plan.path[i], req.landmarks);
00622     // If we have a pan/tilt and we made a large turn, relocalize
00623 #ifdef TGT_HAS_HEAD
00624     if ( fabs(turn) > M_PI / 6 )
00625       if ( req.landmarks.size() > 0)
00626   plan.addNavigationStep(localizeStep, plan.path[i], req.landmarks);
00627 #endif
00628 
00629     // Calculate the number of times to localize during travel, but
00630     // don't try to localize if we have no landmarks.
00631     float maxDistanceBetween = 500;   //!< farthest we're willing to travel before localizing again
00632     //if ( req.landmarks.empty() ) {
00633     //  maxDistanceBetween = 1e10f;
00634     //}
00635     int numberOfLocalizations = (int)floor(distanceBetween / maxDistanceBetween);
00636     Point deltaStep = delta.unitVector() * maxDistanceBetween;
00637     Point currentPoint = Point(plan.path[i-1].x, plan.path[i-1].y, 0, allocentric);
00638     for ( int j=0; j < numberOfLocalizations; j++ ) {
00639       currentPoint += deltaStep;
00640       plan.addNavigationStep(travelStep, currentPoint, plan.path[i].theta, 0, req.landmarks);
00641       if ( req.landmarks.size() > 0)
00642   plan.addNavigationStep(localizeStep, currentPoint, plan.path[i].theta, 0, req.landmarks);
00643     }
00644     plan.addNavigationStep(travelStep, plan.path[i], req.landmarks);
00645   }
00646   AngTwoPi finalHeading = std::isnan((float)req.targetHeading) ? plan.path.back().theta : req.targetHeading;
00647   plan.addNavigationStep(headingStep, Point(), finalHeading, 0, req.landmarks);
00648   plan.currentStep = plan.steps.begin();
00649 
00650   if ( req.planEditor )  // Let user edit the plan if desired
00651     (*req.planEditor)(plan, req);
00652   postStateCompletion();
00653 }
00654 
00655 void Pilot::ExecutePlan::ExecuteStep::doExecute(NavigationPlan &plan, 
00656             DisplacementInstruction &nextDisplacementInstruction,
00657             const bool pushType) {
00658 
00659   //std::cout << "--------ExecutePlan::ExecuteStep::doExecute" << std::endl;
00660   DualCoding::Point position = VRmixin::theAgent->getCentroid();
00661 
00662   if (plan.steps.size() == 0) {
00663     std::cout << "Warning: empty path in Pilot::ExecutePlan::doStart" << std::endl;
00664     postStateCompletion();
00665     return;
00666   }
00667 
00668   nextDisplacementInstruction.walkBackward = plan.walkBackward;
00669   switch (plan.currentStep->type) {
00670 
00671   case localizeStep:
00672     if (!pushType)
00673         postStateSignal<NavigationStepType_t>(localizeStep);
00674     else
00675 #ifdef TGT_HAS_HEAD
00676         postStateSignal<NavigationStepType_t>(localizeStep);
00677 #else
00678         postStateSuccess();
00679 #endif
00680     break;
00681 
00682   case travelStep: {
00683     position = VRmixin::theAgent->getCentroid();
00684     fmat::Column<3> pos = position.getCoords();
00685     fmat::Column<3> next = plan.currentStep->waypoint.getCoords();      
00686     fmat::Column<2> disp = fmat::SubVector<2>(next) - fmat::SubVector<2>(pos);
00687     AngSignPi course = atan2(disp[1], disp[0]);
00688     if ( verbosity & PVnavStep )
00689       cout << ">>> Travel: from " << pos
00690      << " hdg " << float(AngSignPi(VRmixin::theAgent->getOrientation()))*180/M_PI 
00691      << " deg., course " << float(course)*180/M_PI <<" to " << next;
00692     if (position.xyDistanceFrom(plan.currentStep->waypoint) < allowableDistanceError) {
00693       if ( verbosity & PVnavStep )
00694         cout << ".   Close enough!" << endl;
00695       postStateSuccess();
00696     } else {
00697       AngSignPi angle = course - VRmixin::theAgent->getOrientation();
00698       if ( nextDisplacementInstruction.walkBackward ) {
00699         angle += M_PI;
00700         cout << "nextDisplacementInstruction.walkBackward is true" << endl;
00701       }
00702       if (fabs(angle) < allowableAngularError) {
00703         nextDisplacementInstruction.nextPoint = disp;
00704         if ( verbosity & PVnavStep )
00705           cout << ", disp=" << disp << endl;
00706         postStateSignal<NavigationStepType_t>(travelStep);
00707       }
00708       else {
00709         if ( verbosity & PVnavStep )
00710           cout << "... deferred" << endl << ">>> Turn before travel: "
00711                << float(angle)*180/M_PI << " deg" << endl;
00712         nextDisplacementInstruction.angleToTurn = angle;
00713         postStateSignal<NavigationStepType_t>(preTravelStep);
00714       }
00715     }
00716     break;
00717   }
00718 
00719   case turnStep: {
00720     position = VRmixin::theAgent->getCentroid();
00721     fmat::Column<3> pos = position.getCoords();
00722     fmat::Column<3> next = plan.currentStep->waypoint.getCoords();
00723     fmat::Column<2> disp = fmat::SubVector<2>(next) - fmat::SubVector<2>(pos);
00724     AngSignPi directTurn = AngSignPi(atan2(disp[1], disp[0]) - VRmixin::theAgent->getOrientation());
00725     if ( fabs(AngSignPi(directTurn - plan.currentStep->turn)) > M_PI/1.8 )
00726    directTurn += M_PI;  // intent was to walk backwards
00727     AngSignTwoPi actualTurn = float(directTurn);
00728 
00729     // The amount of the turn is calculated dynamically based on our
00730     // actual position.  Its sign may change relative to the planned
00731     // turn due to position error.  But to assure obstacle avoidance,
00732     // if the planned turn amount is more than 90 degrees, the sign is
00733     // unlikely to have changed, and the planned turning direction
00734     // (left or right) will be honored even if we have to turn more
00735     // than 180 degrees.
00736     if ( fabs(plan.currentStep->turn) > M_PI/2 ) {
00737       if ( directTurn > 0 && plan.currentStep->turn < 0 )
00738   actualTurn -= 2*M_PI;
00739       else if ( directTurn < 0 && plan.currentStep->turn > 0 )
00740   actualTurn += 2*M_PI;
00741     }
00742 
00743     if (fabs(actualTurn) < allowableAngularError) {
00744       if ( verbosity & PVnavStep )
00745   cout << ">>> Turn: " << float(actualTurn)*180/M_PI << " deg is smaller than minimum "
00746        << float(allowableAngularError)*180/M_PI << " deg (skipped)" << endl;
00747       postStateSuccess();
00748     } else {
00749       if ( verbosity & PVnavStep )
00750   cout << ">>> Turn: " << float(actualTurn)*180/M_PI << " deg" << endl;
00751       nextDisplacementInstruction.angleToTurn = actualTurn;
00752       postStateSignal<NavigationStepType_t>(turnStep);
00753     }
00754     break;
00755     }
00756 
00757   case headingStep: {
00758     AngSignPi angle = float(plan.currentStep->orientation - VRmixin::theAgent->getOrientation());
00759     if (fabs(angle) < allowableAngularError) {
00760       if ( verbosity & PVnavStep )
00761   cout << ">>> Turn: " << float(angle)*180/M_PI << " deg is smaller than minimum "
00762        << float(allowableAngularError)*180/M_PI << " deg (skipped)" << endl;
00763       postStateSuccess();
00764     } else {
00765       if ( verbosity & PVnavStep )
00766   cout << ">>> Turn: " << float(angle)*180/M_PI 
00767        << " deg to heading " << float(plan.currentStep->orientation)*(180/M_PI) << endl;
00768       nextDisplacementInstruction.angleToTurn = angle;
00769       postStateSignal<NavigationStepType_t>(headingStep);
00770     }
00771     break;
00772   }
00773 
00774   default:
00775     cout << "Incorrect step type " << plan.currentStep->type << "in ExecuteStep" << endl;
00776   }
00777 }
00778 
00779 void Pilot::ExecutePlan::AdjustHeading::doAdjust(NavigationPlan &plan) {
00780   cout << "AdjustHeading: navplan = " << plan.toString() << endl;
00781   DualCoding::Point position = VRmixin::theAgent->getCentroid();
00782   fmat::Column<3> pos = position.getCoords();
00783 
00784   fmat::Column<3> cur, next;
00785   cur = plan.currentStep->waypoint.getCoords();
00786   if (adjustmentType == collisionAdjust) {
00787     next = plan.currentStep->waypoint.getCoords();
00788     cout << "*************** ADJUST TURN COLLISION ***********************************" << endl;
00789   } else {
00790     plan.currentStep++;
00791     next = plan.currentStep->waypoint.getCoords();
00792     plan.currentStep--;
00793   }
00794 
00795   // *** Should adjust for walking backward
00796 
00797   fmat::Column<2> disp = fmat::SubVector<2>(next) - fmat::SubVector<2>(pos);
00798   AngTwoPi newHeading = atan2(disp[1], disp[0]);
00799   AngSignPi turnAngle = float(newHeading - VRmixin::theAgent->getOrientation());
00800   //std::cout << plan << std::endl;
00801   std::cout << "AdjustHeading: pos = " << pos << "  curWP = " << cur << "  nextWP = " << next << std::endl;
00802   std::cout << "AdjustHeading: disp=" << disp << "  new heading " << newHeading
00803             << " (" << float(newHeading)*180/M_PI << " deg.); turn by "
00804             << float(turnAngle)*180/M_PI << " deg." << std::endl;
00805   if ( plan.currentStep != plan.steps.begin() && abs(turnAngle) > 45.0*(M_PI/180.0) ) {
00806     std::cout << "AdjustHeading: overshot target; won't try to correct heading now." << std::endl;
00807     postStateCompletion();
00808     return;
00809   }
00810   if ( (disp.norm() < 50.0 && abs(turnAngle) > 5.0*(M_PI/180.)) ||
00811        abs(turnAngle) < 5.0*(M_PI/180.) ) {
00812     std::cout << "AdjustHeading: punting insignificant turn." << std::endl;
00813     postStateCompletion();
00814     return;
00815   }
00816   PilotRequest &curReq = *VRmixin::pilot->curReq;
00817 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00818   float va = (curReq.turnSpeed > 0) ? curReq.turnSpeed : 0.25f;
00819 #else
00820   // these values are for Chiara
00821   float va = (curReq.turnSpeed > 0) ? curReq.turnSpeed : 0.1f;
00822 #endif
00823   getMC()->setTargetDisplacement(0,0,turnAngle,0,0,va);
00824   motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
00825 }
00826 
00827 
00828 
00829 //================ LocalizationUtility ================
00830 
00831 const float Pilot::LocalizationUtility::Localize::minConfidentWeight = -30;
00832 
00833 #ifdef TGT_HAS_HEAD
00834 void Pilot::LocalizationUtility::ChooseGazePoints::setupGazePoints(std::deque<Point> &gazePoints) {
00835   std::vector<ShapeRoot> visibleLandmarks =
00836     calculateVisibleLandmarks(VRmixin::theAgent->getCentroid(), VRmixin::theAgent->getOrientation(), 
00837             M_PI*(2.0/3.0), // assume head can turn 120 degrees left or right
00838             VRmixin::pilot->curReq->landmarks);
00839   gazePoints = std::deque<Point>();
00840   for ( unsigned int i = 0; i < visibleLandmarks.size(); i++ ) {
00841     // convert to egocentric coordinates
00842     Point allovec = visibleLandmarks[i]->getCentroid() - VRmixin::theAgent->getCentroid();
00843     Point egovec = Point(fmat::rotationZ(- VRmixin::theAgent->getOrientation()) * allovec.getCoords());
00844     egovec.setRefFrameType(egocentric);
00845     gazePoints.push_back(egovec);
00846   }
00847   std::cout << "Pilot wants to localize: " << gazePoints.size() << " gaze points." << std::endl;
00848 }
00849 
00850 #else  // robot has no head, so plan on turning the body
00851 void Pilot::LocalizationUtility::ChooseGazePoints::setupGazeHeadings
00852                 (const AngTwoPi initialHeading, std::deque<AngTwoPi> &gazeHeadings, const AngTwoPi maxTurn) {
00853   const AngTwoPi robotHeading = VRmixin::theAgent->getOrientation();
00854   std::vector<ShapeRoot> visibleLandmarks =
00855     calculateVisibleLandmarks(VRmixin::theAgent->getCentroid(), robotHeading, 
00856             maxTurn, // assume body can turn the full +/- 180 degrees
00857             VRmixin::pilot->curReq->landmarks);
00858   gazeHeadings = std::deque<AngTwoPi>();
00859   for ( unsigned int i = 0; i < visibleLandmarks.size(); i++ ) {
00860     Point allovec = visibleLandmarks[i]->getCentroid() - VRmixin::theAgent->getCentroid();
00861     gazeHeadings.push_back(AngTwoPi(allovec.atanYX()));
00862   }
00863   // Landmark headings are allocentric, but to minimize turn distance
00864   // we want to sort them by their bearing relative to the robot.
00865   std::sort(gazeHeadings.begin(), gazeHeadings.end(), BearingLessThan(robotHeading));
00866   // Reverse the list if the closest landmark is to the right instead of the left
00867   if ( gazeHeadings.size() > 1 &&
00868        abs(AngSignPi(gazeHeadings.front()-robotHeading)) > abs(AngSignPi(gazeHeadings.back()-robotHeading)) )
00869     reverse(gazeHeadings.begin(), gazeHeadings.end());
00870 }
00871 
00872 void Pilot::LocalizationUtility::PrepareForNextGazePoint::prepareForNext(std::deque<AngTwoPi> &gazeHeadings, const AngTwoPi maxTurn) {
00873   if ( gazeHeadings.empty() ) {
00874     postStateFailure();
00875     return;
00876   }
00877   const AngTwoPi curHeading = VRmixin::theAgent->getOrientation();
00878   const AngTwoPi newHeading = gazeHeadings.front();
00879   gazeHeadings.pop_front();
00880   const AngSignPi turnAmount = AngSignPi(newHeading - curHeading);
00881   // std::cout << "*** curHeading=" << curHeading << "  newHeading=" << newHeading
00882   //          << "  turnAmount=" << turnAmount << "  maxturn=" << maxTurn << std::endl;
00883   if (fabs(turnAmount) > maxTurn) {
00884     postStateSuccess();
00885     return;
00886   }
00887   MMAccessor<WalkMC> walk_acc(getMC());
00888   float va = VRmixin::pilot->curReq->turnSpeed;
00889   if ( va == 0 )
00890     va = 1.0f;   // good turn speed for CREATE odometry
00891   walk_acc->setTargetDisplacement(0, 0, turnAmount, 0, 0, va);
00892   motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
00893 }
00894 
00895 #endif // TGT_HAS_HEAD
00896 
00897   void Pilot::LocalizationUtility::Localize::doStart() {
00898     VRmixin::particleFilter->updateSensors(VRmixin::particleFilter->getSensorModel(),false,true);
00899     const ShapeBasedParticleFilter::particle_type &estimate = VRmixin::particleFilter->getEstimate();
00900     if ( estimate.weight < minConfidentWeight ) {
00901       std::cout << "  Particle filter bestWeight=" << estimate.weight
00902     << " < minConfidentWeight=" << minConfidentWeight << ": resample." << std::endl;
00903       VRmixin::particleFilter->updateSensors(VRmixin::particleFilter->getSensorModel(),false,true);
00904     }
00905     if ( estimate.weight < minConfidentWeight ) {
00906       std::cout << "  Particle filter bestWeight=" << estimate.weight
00907     << " < minConfidentWeight=" << minConfidentWeight << ": resample once more." << std::endl;
00908       VRmixin::particleFilter->updateSensors(VRmixin::particleFilter->getSensorModel(),false,true);
00909     }
00910     VRmixin::mapBuilder->setAgent(Point(estimate.x,estimate.y,0,allocentric), estimate.theta, true);
00911     AngTwoPi heading = VRmixin::theAgent->getOrientation();
00912     cout << "Agent now at " << VRmixin::theAgent->getCentroid() << " hdg " << heading
00913          << " (= " << float(heading)*180/M_PI << " deg.)" 
00914          << "  std. dev. " << sqrt(((ShapeBasedParticleFilter*)VRmixin::particleFilter)->getVariance().x) << " mm, "
00915          << float(((ShapeBasedParticleFilter*)VRmixin::particleFilter)->getVariance().theta) * 180 / M_PI << " deg."
00916          << "  wtvar " << ((ShapeBasedParticleFilter*)VRmixin::particleFilter)->getVariance().y
00917          << "  bestWeight=" << estimate.weight
00918          << endl;
00919 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00920     // cout << "GPS = [ " << state->sensors[GPSXOffset] << " , " << state->sensors[GPSYOffset] << " ]" << endl;
00921 #endif
00922     if ( VRmixin::pilot->curReq->displayIndividualParticles > 0 )
00923       VRmixin::particleFilter->displayIndividualParticles(VRmixin::pilot->curReq->displayIndividualParticles);
00924     else if ( VRmixin::pilot->curReq->displayParticles > 0 )
00925       VRmixin::particleFilter->displayParticles(VRmixin::pilot->curReq->displayParticles);
00926     else {
00927       GET_SHAPE(particles, GraphicsData, VRmixin::worldShS);
00928       if ( particles.isValid() )
00929   VRmixin::particleFilter->displayParticles(50);
00930     }
00931     VRmixin::autoRefreshSketchWorld();
00932   }
00933 
00934 //================ WalkMachine ================
00935 
00936 void Pilot::WalkMachine::WalkMachine_Walker::doStart() {
00937   PilotRequest &preq = *VRmixin::pilot->curReq;
00938   MMAccessor<WalkMC> walk_acc(getMC());
00939 #ifdef TGT_HAS_WHEELS
00940   float vx = (preq.forwardSpeed > 0) ? preq.forwardSpeed : 50.f;
00941   float vy = 0;
00942   float va = (preq.turnSpeed > 0) ? preq.turnSpeed : 0.25f;
00943 #else
00944   // these values are for Chiara
00945   float vx = (preq.forwardSpeed > 0) ? preq.forwardSpeed : 15.f;
00946   float vy = preq.strafeSpeed;
00947   float va = (preq.turnSpeed > 0) ? preq.turnSpeed : 0.1f;
00948 #endif
00949   std::cout << "Pilot walking:";
00950   if ( preq.dx != 0 )
00951     std::cout << " dx = " << preq.dx;
00952   if ( preq.dy != 0 )
00953     std::cout << " dy = " << preq.dy;
00954   if ( preq.da != 0 )
00955     std::cout << " da = " << preq.da << " (" << preq.da*180/M_PI << " deg.)";
00956   std::cout << endl;
00957   walk_acc->setTargetDisplacement(preq.dx, preq.dy, preq.da, vx, vy, va);
00958   motman->setPriority(VRmixin::pilot->getWalk_id(),MotionManager::kStdPriority);
00959 }
00960 
00961 
00962 //================ WaypointWalkmachine ================
00963 
00964 void Pilot::WaypointWalkMachine::WaypointWalkMachine_WaypointWalker::doStart() {
00965   PilotRequest &req = *VRmixin::pilot->curReq;
00966   MMAccessor<WaypointWalkMC> wp_acc(getMC());
00967   if (req.clearWaypoints)
00968     wp_acc->clearWaypointList();
00969   if ( req.waypointList.size() > 0 )
00970     wp_acc->appendWaypoints(req.waypointList);
00971   else
00972     std::cout << "Warning: Pilot request to waypointWalk with empty waypoint list." << std::endl;
00973   motman->setPriority(VRmixin::VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
00974 }
00975 
00976 //================ SetVelocityMachine ================
00977 
00978 void Pilot::SetVelocityMachine::SetVelocityMachine_Walker::doStart() {
00979   PilotRequest &preq = *VRmixin::pilot->curReq;
00980   MotionManager::MC_ID wid = VRmixin::pilot->getWalk_id();
00981   MMAccessor<WalkMC>(wid)->setTargetVelocity(preq.forwardSpeed, preq.strafeSpeed, preq.turnSpeed);
00982   motman->setPriority(wid,MotionManager::kStdPriority);
00983 }
00984 
00985 void Pilot:: changeVelocity(float vx, float vy, float va) {
00986   if ( VRmixin::pilot->curReq == NULL ) {
00987     std::cout << "Warning: call to Pilot::changeVelocity() when no Pilot request is active: ignored." << std::endl;
00988     return;
00989   }
00990   PilotRequest &preq = *VRmixin::pilot->curReq;
00991   switch ( preq.requestType ) {
00992   case PilotTypes::walk:
00993   case PilotTypes::setVelocity:
00994     {
00995       MotionManager::MC_ID wid = VRmixin::pilot->getWalk_id();
00996       if ( wid == MotionManager::invalid_MC_ID )
00997         cout << "Error: Pilot::changeVelocity() tried to use an invalid MC_ID" << endl;
00998       else
00999         MMAccessor<WalkMC>(wid)->setTargetVelocity(vx, vy, va);
01000       break;
01001     }
01002   default:
01003     {
01004       std::cout << "Warning: Pilot::changeVelocity() called, but the active Pilot request is not 'walk' or 'setVelocity': ignored."
01005                 << std::endl;
01006     }
01007   }
01008 }
01009 
01010 void Pilot::cancelVelocity() {
01011   if ( VRmixin::pilot->curReq != NULL && VRmixin::pilot->curReq->requestType == PilotTypes::setVelocity ) {
01012     MotionManager::MC_ID wid = VRmixin::pilot->getWalk_id();
01013     erouter->postEvent(EventBase(EventBase::motmanEGID, wid, EventBase::statusETID, 0));  // force WalkNode to complete
01014   }
01015   else
01016     std::cout << "Warning: call to Pilot::cancelVelocity() when not executing a setVelocity request: ignored." << std::endl;
01017 }
01018 
01019 //================ GoToShapeMachine ================
01020 
01021 const float Pilot::ExecutePlan::allowableDistanceError = 100.f;
01022 const float Pilot::ExecutePlan::allowableAngularError = 0.2f;
01023 
01024 void Pilot::GoToShapeMachine::CheckParameters::doStart() {
01025 
01026   PilotRequest &req = *VRmixin::pilot->curReq;
01027   if ( ! req.targetShape.isValid() ) {
01028     cout << "Pilot request goToShape fails: no valid targetShape specified" << endl;
01029     postStateFailure();
01030     return;
01031   }
01032   if ( req.targetShape->isObstacle() ) {
01033     cout << "Warning: Pilot marking targetShape '" << req.targetShape->getName()
01034    << "' (" << req.targetShape->getId() << ") as non-obstacle to permit path planning." << endl;
01035     req.targetShape->setObstacle(false);
01036   }
01037   // arguments look good; okay to proceed
01038   postStateSuccess();
01039 }
01040 
01041 //================PushObjectMachine================
01042 void Pilot::PushObjectMachine::CheckParameters::doStart() {
01043 
01044   PilotRequest &req = *VRmixin::pilot->curReq;
01045   if ( ! req.objectShape.isValid() ) {
01046     cout << "Pilot request pushObject fails: no valid objectShape specified" << endl;
01047     postStateFailure();
01048     return;
01049   if ( ! req.targetShape.isValid() ) {
01050     cout << "Pilot request pushObject fails: no valid targetShape specified" << endl;
01051     postStateFailure();
01052     return;
01053   }
01054   if ( req.targetShape->isObstacle() ) {
01055     cout << "Warning: Pilot marking targetShape '" << req.targetShape->getName()
01056    << "' (" << req.targetShape->getId() << ") as non-obstacle to permit path planning." << endl;
01057     req.targetShape->setObstacle(false);
01058   }
01059   }
01060   // arguments look good; okay to proceed
01061   postStateSuccess();
01062 }
01063 
01064 
01065 //=== Pilot static variables 
01066   unsigned int Pilot::verbosity = -1U & ~Pilot::PVnavStep;
01067   
01068 //=== PushObjectMachine static variables 
01069   int Pilot::PushObjectMachine::backupDist = 600;
01070   int Pilot::PushObjectMachine::obstDist = 150;
01071   int Pilot::PushObjectMachine::robotDiam = 150;
01072   int Pilot::PushObjectMachine::objSize = 50;
01073   float Pilot::PushObjectMachine::prePushTurnSpeed = 0.25f;
01074   float Pilot::PushObjectMachine::prePushForwardSpeed = 100.f;
01075   float Pilot::PushObjectMachine::pushTurnSpeed = 0.25f;
01076   float Pilot::PushObjectMachine::pushForwardSpeed = 100.f;
01077 } // namespace
01078 
01079 #endif

DualCoding 5.1CVS
Generated Mon May 9 04:56:27 2016 by Doxygen 1.6.3