Pilot.cc

Go to the documentation of this file.

#include "Motion/WalkMC.h"
#ifdef TGT_HAS_WALK

#include <vector>

#include "Crew/Pilot.h"
#include "DualCoding/VRmixin.h"
#include "Events/PilotEvent.h"
#include "Shared/mathutils.h" // for isnan fix

namespace DualCoding {

void Pilot::doStart() {
  if (verbosity & PVstart)
    cout << "Pilot starting up: walk_id= " << walk_id <<"  waypointwalk_id= " << waypointwalk_id << endl;
  setAgent(Point(0,0,0,allocentric),0,false,false);
  VRmixin::particleFilter->displayParticles(50);
  addNode(new RunMotionModel)->start();
  addNode(new CollisionChecker)->start();
}

void Pilot::doStop() {
  pilotAbort();
  if (verbosity & PVstart)
    cout << "Pilot is shutting down." << endl;
  motman->removeMotion(walk_id);
  motman->removeMotion(waypointwalk_id);
  waypointwalk_id = MotionManager::invalid_MC_ID;
  walk_id = MotionManager::invalid_MC_ID;
  delete defaultLandmarkExtractor;
  defaultLandmarkExtractor = NULL;

  // The following teardown() call is necessary because setup
  // hard-wires the MC_ID for all the walk nodes.  If we leave the
  // waypoint walk motion command active when Tekkotsu shuts down,
  // it crashes and generates a nasty backtrace.  But if we remove
  // the motion command, there's no easy way to put it back because
  // setup won't be called again unless we do this teardown() call.
  teardown();
}

void Pilot::doEvent() {
  if (event->getGeneratorID() == EventBase::timerEGID && event->getSourceID() == 9999)
    executeRequest();
  else
    cout << "Pilot got unexpected event: " << event->getDescription() << endl;
}

void Pilot::unwindForNextRequest() {
  erouter->addTimer(this,9999,1,false); // allow time to unwind event stack before executing next request
}

void Pilot::setAgent(const Point &loc, AngTwoPi heading, bool quiet, bool updateWaypoint) {
  if (updateWaypoint) {
    cout << "Updating waypoint walk to " << loc << " hdg=" << heading << endl;
    MMAccessor<WaypointWalkMC> wp_acc(waypointwalk_id);
    wp_acc->setCurPos(loc.coordX(), loc.coordY(), heading);
  }
  VRmixin::mapBuilder->setAgent(loc, heading, quiet);
  // Synching the estimate (below) is a no-op if setAgent was
  // invoked by Pilot::RunMotionModel, but not if the user called
  // setAgent directly.
  VRmixin::particleFilter->setPosition(loc.coordX(), loc.coordY(), heading);
  VRmixin::particleFilter->synchEstimateToAgent();
}

unsigned int Pilot::executeRequest(BehaviorBase* requestingBehavior, const PilotRequest& req) {
  requests.push(new PilotRequest(req));
  const unsigned int reqID = ++idCounter;
  requests.back()->requestID = reqID;
  requests.back()->requestingBehavior = requestingBehavior;
  if (curReq == NULL)
    unwindForNextRequest();
  else if (verbosity & PVqueued)
    cout << "Pilot request " << requests.back()->requestID << " queued." << endl;
  return reqID;
}


void Pilot::executeRequest() {
  if (curReq == NULL && !requests.empty()) {
    curReq = requests.front();
    if (verbosity & PVexecute)
      cout << "Pilot executing request " << curReq->requestID
     << ": " << PilotTypes::RequestTypeNames[curReq->requestType] << endl;
    VRmixin::particleFilter->synchEstimateToAgent();
    if ( ! curReq->walkParameters.empty() ) {
      MMAccessor<WaypointWalkMC>(VRmixin::pilot->getWaypointwalk_id())->WalkMC::loadFile(curReq->walkParameters.c_str());
      MMAccessor<WalkMC>(VRmixin::pilot->getWalk_id())->loadFile(curReq->walkParameters.c_str());
    }
    dispatch_->start();
  }
}

void Pilot::requestComplete(PilotTypes::ErrorType_t errorType) {
  if (curReq == NULL) {
    cout << "Pilot::requestComplete had NULL curReq !!!!!!!!!!!!!!!!" << endl;
    return;
  }
  const BehaviorBase* requester = curReq->requestingBehavior;
  PilotEvent e(EventBase::pilotEGID,
         (requester==NULL) ? curReq->requestID : (size_t)requester,
         EventBase::statusETID);
  e.requestType = curReq->requestType;
  e.errorType = errorType;
  if (verbosity & PVcomplete)
    cout << "Pilot request " << curReq->requestID << " complete: "
   << PilotTypes::ErrorTypeNames[errorType] <<  endl;
  requestComplete(e);
}

void Pilot::requestComplete(const PilotEvent &e) {
  //if ( curReq->trackRequest != NULL )
  //  VRmixin::lookout.stopTrack();
  dispatch_->finish();
  // now clean up everything before posting the event
  delete curReq->landmarkExtractor;
  delete curReq->searchObjectExtractor;
  delete curReq;
  curReq = NULL;
  requests.pop();
  // post the event and allow time to unwind before starting on next event in the queue
  erouter->postEvent(e);
  unwindForNextRequest();
}

void Pilot::setWorldBounds(const Shape<PolygonData> &bounds) {
  VRmixin::particleFilter->setWorldBounds(bounds);
}

void Pilot::setDefaultLandmarks(const std::vector<ShapeRoot> &landmarks) {
  defaultLandmarks = landmarks;
}

void Pilot::setDefaultLandmarkExtractor(const MapBuilderRequest &mapreq) {
  defaultLandmarkExtractor = new MapBuilderRequest(mapreq);
  defaultLandmarkExtractor->clearLocal = false;
}

void Pilot::pilotPop() {
  if ( curReq != NULL ) {
    if ( verbosity & PVpop )
      cout << "Pilot popping current request." << endl;
    dispatch_->stop();
    VRmixin::pilot->requestComplete(PilotTypes::abort);
  }
}

void Pilot::pilotAbort() {
  if ( curReq != NULL && (verbosity & PVabort) )
    cout << "Pilot aborting." << endl;
  dispatch_->stop();
  curReq = NULL;
  while (!requests.empty()) {
    delete requests.front();
    requests.pop();
  }
}

void Pilot::setupLandmarkExtractor() {

  if ( curReq->landmarkExitTest == NULL )
    curReq->landmarkExitTest = &Pilot::defaultLandmarkExitTest;
  if ( curReq->landmarkExtractor != NULL )  // user supplied his own, so go with that
    return;
  else if ( defaultLandmarkExtractor != NULL ) {
    curReq->landmarkExtractor = new MapBuilderRequest(*defaultLandmarkExtractor);
    return;
  }
  vector<ShapeRoot> landmarkTemp;
  if ( ! curReq->landmarks.empty() )
    landmarkTemp = curReq->landmarks;
  else if ( ! defaultLandmarks.empty() )
    landmarkTemp = defaultLandmarks;
  else {  // the user specified no landmarks, so look in worldShS for possibilities

    NEW_SHAPEVEC(markers, MarkerData, select_type<MarkerData>(VRmixin::worldShS));
    landmarkTemp.insert(landmarkTemp.begin(), markers.begin(), markers.end());
    NEW_SHAPEVEC(apriltags, AprilTagData, select_type<AprilTagData>(VRmixin::worldShS));
    landmarkTemp.insert(landmarkTemp.begin(), apriltags.begin(), apriltags.end());
    curReq->landmarks = landmarkTemp;  // save what we found so the navigation planner can use them
  }
  if ( !landmarkTemp.empty() ) {
    curReq->landmarkExtractor = new MapBuilderRequest(MapBuilderRequest::localMap);
    curReq->landmarkExtractor->clearVerbosity = -1U;
    curReq->landmarkExtractor->setVerbosity = MapBuilder::MBVimportShapes;
    if ( ! select_type<AprilTagData>(landmarkTemp).empty() )
      curReq->landmarkExtractor->setAprilTagFamily();  // HACK: should be using the tag's actual family, not default family, but we don't currently store that info
    // iterate through landmarkTemp and add all the marker types to landmarkExtractor
    for ( vector<ShapeRoot>::const_iterator it = landmarkTemp.begin();
    it != landmarkTemp.end(); it++ ) {
      if ( (*it)->getType() == markerDataType ) {
  const Shape<MarkerData> &m = ShapeRootTypeConst(*it, MarkerData);
  curReq->landmarkExtractor->addMarkerType(m->getMarkerType());
  curReq->landmarkExtractor->objectColors[markerDataType].insert(ProjectInterface::getColorIndex(m->getColor()));
      }
    }
  }
}

bool Pilot::defaultLandmarkExitTest() {
  return ( VRmixin::localShS.allShapes().size() >= 2 );
}

std::vector<ShapeRoot>
  Pilot::calculateVisibleLandmarks(const DualCoding::Point &currentLocation,
           AngTwoPi currentOrientation, AngTwoPi maxTurn,
           const std::vector<DualCoding::ShapeRoot> &possibleLandmarks) {
  std::vector<ShapeRoot> result;
  for(unsigned int i = 0; i<possibleLandmarks.size(); i++)
    if ( isLandmarkViewable(possibleLandmarks[i], currentLocation, currentOrientation, maxTurn) )
      result.push_back(possibleLandmarks[i]);
  return result;
}

bool Pilot::isLandmarkViewable(const DualCoding::ShapeRoot &selectedLandmark,
             const DualCoding::Point &currentLocation,
             AngTwoPi currentOrientation, AngTwoPi maxTurn) {
  AngSignPi bearing = (selectedLandmark->getCentroid() - currentLocation).atanYX();

  // is the camera within our possible field of view (in front of us)?
  if ( fabs(bearing) > (CameraHorizFOV/2.0+maxTurn) )
    return false;

  // If there are no walls, assume all landmarks are visible,
  // but this isn't really true for poster-type markers where
  // we need to know the viewing angle.  Will fix this someday.
  Shape<PolygonData> boundary;   // either worldBounds or wall
  GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
  if ( worldBounds.isValid() )
    boundary = worldBounds;
  else {
    GET_SHAPE(wall, PolygonData, VRmixin::worldShS);
    boundary = wall;
  }
  if ( ! boundary.isValid() )
    return true;
  // do we intersect with the world bounds when looking at marker?
  LineData lineOfSight(VRmixin::worldShS, currentLocation, selectedLandmark->getCentroid());
  if ( boundary->intersectsLine(lineOfSight) )
    return false;

  // this landmark is viewable
  return true;
}

void Pilot::CollisionDispatch::doStart() {
  switch ( VRmixin::pilot->curReq->collisionAction ) {
  case collisionStop:
  case collisionReplan:
    erouter->addListener(this, EventBase::pilotEGID, (size_t)VRmixin::pilot, EventBase::statusETID);
    break;
  default:
    break;
  }
}

void Pilot::CollisionDispatch::doEvent() {
  // If we're listening for Pilot collisions then we want to stop or
  // replan, so rebroadcast with this as the source node to trigger
  PilotEvent e(*event);
  e.setSourceID((size_t)this);
  erouter->postEvent(e);
}

void Pilot::TerminateDueToCollision::doStart() {
  PilotEvent e(*event);  // copy and convert from an internal Pilot event to a public one
  e.setSourceID((size_t)VRmixin::pilot->curReq->requestingBehavior);
  VRmixin::pilot->requestComplete(e);
}

//================ Background processes ================

void Pilot::RunMotionModel::doStart() {
  erouter->addTimer(this, 1, 250, true);  // do odometry update every 250 msec
}

void Pilot::RunMotionModel::doEvent() {
  if (event->getGeneratorID() == EventBase::timerEGID) {
    VRmixin::particleFilter->updateMotion();
    LocalizationParticle estimate = VRmixin::particleFilter->getEstimate();
    VRmixin::mapBuilder->setAgent(Point(estimate.x, estimate.y, 0, allocentric), estimate.theta, true);
  }
}

void Pilot::CollisionChecker::doStart() {
  enableDetection();
}

void Pilot::CollisionChecker::enableDetection() {
#ifdef TGT_IS_CREATE
  // std::cout << "Enabled collision detection." << std::endl;
  erouter->addListener(this, EventBase::buttonEGID, BumpLeftButOffset, EventBase::activateETID);
  erouter->addListener(this, EventBase::buttonEGID, BumpRightButOffset, EventBase::activateETID);
  erouter->addListener(this, EventBase::buttonEGID, OvercurrentLeftWheelOffset, EventBase::activateETID);
  erouter->addListener(this, EventBase::buttonEGID, OvercurrentRightWheelOffset, EventBase::activateETID);
  erouter->addTimer(this, overcurrentResetTimer, 1500, true); // reset overcurrentCounter frequently
#else
  std::cout << "Warning: Pilot has no collision check method defined for this robot type" << std::endl;
#endif
}

void Pilot::CollisionChecker::disableDetection(unsigned int howlong=-1U) {
#ifdef TGT_IS_CREATE
  // std::cout << "Disabling collision detection for " << howlong << " ms." << std::endl;
  erouter->removeListener(this, EventBase::buttonEGID);
#endif
  erouter->addTimer(this, 1, howlong, false);
}

void Pilot::CollisionChecker::doEvent() {
  if ( event->getGeneratorID() == EventBase::timerEGID ) {
    if ( event->getSourceID() == collisionEnableTimer )
      enableDetection();
#ifdef TGT_IS_CREATE
    else if ( event->getSourceID() == overcurrentResetTimer )
      overcurrentCounter = 0;
    return;
#endif
  }
  // If we get here, event must have been a button press (bump sesnsor or overcurrent)
  if ( VRmixin::pilot->curReq == NULL || 
       VRmixin::pilot->curReq->collisionAction == collisionIgnore )
    return;
#ifdef TGT_IS_CREATE
  if ( event->getSourceID() == BumpLeftButOffset ||
       event->getSourceID() == BumpRightButOffset )
    reportCollision();
  else // overcurrent -- could be transient.  Count 'em and see.
    if ( ++overcurrentCounter >= 12 ) {
      overcurrentCounter = 0;
      reportCollision();
    }
#endif
}

void Pilot::CollisionChecker::reportCollision() {
  if ( VRmixin::pilot->verbosity & PVcollision )
    cout << "Pilot: collision detected!" << endl;
  disableDetection(1000); // ignore any further collisions for the next second
  PilotEvent e(EventBase::pilotEGID, (size_t)VRmixin::pilot, EventBase::statusETID);
  e.errorType = collisionDetected;
  // Note: in the future we might want to add info about where on the
  // robot's body the collision occurred.
  erouter->postEvent(e);
}


//================ Navigation plans and steps ================

std::string Pilot::NavigationPlan::toString() const {
  ostringstream os;
  os << "Navigation plan (" << steps.size() << " steps):" << std::endl;
  for ( size_t i=0; i<steps.size(); i++ )
    os << "  " << steps[i].toString() << endl;
  return os.str();
}

void Pilot::NavigationPlan::addNavigationStep(NavigationStepType_t type, const DualCoding::Point &waypoint, AngTwoPi orientation) {
  NavigationStep step(type, waypoint);
  if ( type == localizeStep )
    step.visibleLandmarks = calculateVisibleLandmarks(waypoint, orientation, M_PI/2, VRmixin::pilot->curReq->landmarks);
  if ( type != localizeStep || !step.visibleLandmarks.empty() )
    steps.push_back(step);
}

std::string Pilot::NavigationStep::toString() const {
  ostringstream os;
  os << "NavigationStep[";
  switch (type) {
  case localizeStep: os << "localizeStep"; break;
  case turnStep: os << "turnStep"; break;
  case travelStep: os << "travelStep"; break;
  case turnObjStep: os << "turnObjStep"; break;
  case acquireObjStep: os << "acquireObjStep"; break;
  default:
    os << "unknown step type";
  }
  if ( type == localizeStep ) {
    os << ", visible=(";
    for (size_t i = 0; i < visibleLandmarks.size(); i++)
      os << ( (i>0) ? "," : "")  << visibleLandmarks[i]->getName();
    os << ")";
  } else
    os << ", waypoint=" << waypoint;
  os << "]";
  return os.str();
}

std::ostream& operator<<(std::ostream& os, const Pilot::NavigationStep &step) { return os << step.toString(); }
std::ostream& operator<<(std::ostream& os, const Pilot::NavigationPlan &plan) { return os << plan.toString(); }


void Pilot::PlanPath::doPlan(NavigationPlan &plan, Point initPos, AngTwoPi initHead, PilotRequest req) {
  plan.path.clear();
  {
    GET_SHAPE(plannedPath, GraphicsData, worldShS);
    plannedPath.deleteShape();
    GET_SHAPE(plannerTrees, GraphicsData, worldShS);
    plannerTrees.deleteShape();
  }
  GET_SHAPE(worldBounds, PolygonData, worldShS);

  const ShapeRoot &target = req.targetShape;
  Point targetLoc = target->getCentroid();
  if ( targetLoc.getRefFrameType() == egocentric ) {
    targetLoc.applyTransform(VRmixin::mapBuilder->localToWorldMatrix);
    targetLoc.setRefFrameType(allocentric);
  }

  const bool planForHeading = ! std::isnan((float)req.targetHeading);
  GenericRRTBase::PlannerResult<2> result;
  void *pathResult;
  size_t pathResultSize;
  if ( !planForHeading ) {
    std::cout << "Planning path from " << initPos << " to point: " << targetLoc << std::endl;
    float const robotRadius = 6 * 25.4f * 1.25;   // ***HACK*** should be pulling this from RobotInfo
    ShapeSpacePlannerXY planner(worldShS, worldBounds, req.obstacleInflation, robotRadius);
    if ( req.displayObstacles )
      planner.addObstaclesToShapeSpace(worldShS);
    pathResult = new std::vector<ShapeSpacePlannerXY::NodeValue_t>;
    std::vector<ShapeSpacePlannerXY::NodeType_t> *treeStartResult =
      req.displayTree ? new std::vector<ShapeSpacePlannerXY::NodeType_t> : NULL;
    std::vector<ShapeSpacePlannerXY::NodeType_t> *treeEndResult =
      req.displayTree ? new std::vector<ShapeSpacePlannerXY::NodeType_t> : NULL;
    result = planner.planPath(initPos, targetLoc, req.maxRRTIterations,
            (std::vector<ShapeSpacePlannerXY::NodeValue_t>*)pathResult, treeStartResult, treeEndResult);
    pathResultSize = ((std::vector<ShapeSpacePlannerXY::NodeValue_t>*)pathResult)->size();
    if ( req.displayTree ) {
      NEW_SHAPE(plannerTrees, GraphicsData, new GraphicsData(worldShS));
      planner.plotTree(*treeStartResult, plannerTrees, rgb(0,0,0));
      planner.plotTree(*treeEndResult, plannerTrees, rgb(0,192,0));
      delete treeStartResult;
      delete treeEndResult;
    }
  } else {

    // Plan for target heading
    std::cout << "Planning path from " << initPos << " to point " << targetLoc
              << " heading " << float(req.targetHeading)/M_PI*180 << " deg." << std::endl;
    float gateDistance = 6 * 25.4;  // half a robot width
    Point gateLoc = targetLoc - Point(cos(req.targetHeading),sin(req.targetHeading),0,allocentric) * gateDistance;
    ShapeSpacePlannerXYTheta planner(worldShS, worldBounds, req.obstacleInflation);
    if ( req.displayObstacles )
      planner.addObstaclesToShapeSpace(worldShS);
    pathResult = new std::vector<ShapeSpacePlannerXYTheta::NodeValue_t>;
    std::vector<ShapeSpacePlannerXYTheta::NodeType_t> *treeStartResult = req.displayTree ? new std::vector<ShapeSpacePlannerXYTheta::NodeType_t> : NULL;
    std::vector<ShapeSpacePlannerXYTheta::NodeType_t> *treeEndResult = req.displayTree ? new std::vector<ShapeSpacePlannerXYTheta::NodeType_t> : NULL;
    result = planner.planPath(initPos, gateLoc, initHead, req.targetHeading, req.maxRRTIterations,
            (std::vector<ShapeSpacePlannerXYTheta::NodeValue_t>*)pathResult, 
            treeStartResult, treeEndResult);
    ((std::vector<ShapeSpacePlannerXYTheta::NodeValue_t>*)pathResult)->
      push_back(ShapeSpacePlannerXYTheta::NodeValue_t(std::pair<float,float>(targetLoc.coordX(),targetLoc.coordY()),req.targetHeading));
    pathResultSize = ((std::vector<ShapeSpacePlannerXYTheta::NodeValue_t>*)pathResult)->size();
    if ( req.displayTree ) {
      NEW_SHAPE(plannerTrees, GraphicsData, new GraphicsData(worldShS));
      planner.plotTree(*treeStartResult, plannerTrees, rgb(0,0,0));
      planner.plotTree(*treeEndResult, plannerTrees, rgb(0,192,0));
      delete treeStartResult;
      delete treeEndResult;
    }
  }

  if ( result.code != GenericRRTBase::SUCCESS ) {
    switch ( result.code ) {
    case GenericRRTBase::START_COLLIDES:
      std::cout << "Navigation path planning failed: start state " << result.movingObstacle->toString()
    << " is in collision with " << result.collidingObstacle->toString() << ".\n";
      break;
    case GenericRRTBase::END_COLLIDES:
      std::cout << "Navigation path planning failed: end state " << result.movingObstacle->toString()
    << " is in collision with " << result.collidingObstacle->toString() << ".\n";
      break;
    case GenericRRTBase::MAX_ITER:
      std::cout << "Navigation path planning failed: max iterations " << req.maxRRTIterations << " exceeded.\n";
      break;
    default: break; // dummy to suppress compiler warning
    }
    postStateSignal<GenericRRTBase::PlanPathResultCode>(result.code);
    return;
  }

  // Extract the path into a vector of points
  for ( size_t i = 0; i < pathResultSize; i++ ) {
    const std::pair<float,float> &pt = ( !planForHeading )
      ? (*(std::vector<ShapeSpacePlannerXY::NodeValue_t>*)pathResult)[i]
      : (*(std::vector<ShapeSpacePlannerXYTheta::NodeValue_t>*)pathResult)[i].first;
    plan.path.push_back(Point(pt.first, pt.second, 0, allocentric));
    if ( verbosity & PVshowPath ) {
      cout << "path[" << i << "] = " << plan.path[i];
      if ( planForHeading )
  cout << " hdg " << float(AngSignPi((*(std::vector<ShapeSpacePlannerXYTheta::NodeValue_t>*)pathResult)[i].second)) * 180/M_PI
       << " deg.";
      cout << endl;
    }
  }
  
  // Display the path if requested
  if ( req.displayPath ) {
    NEW_SHAPE(plannedPath, GraphicsData, new GraphicsData(worldShS));
    if ( !planForHeading ) 
      ShapeSpacePlannerXY::plotPath((*(std::vector<ShapeSpacePlannerXY::NodeValue_t>*)pathResult),
            plannedPath, rgb(0,0,255));
    else
      ShapeSpacePlannerXYTheta::plotPath((*(std::vector<ShapeSpacePlannerXYTheta::NodeValue_t>*)pathResult),
           plannedPath, rgb(0,0,255));
  }
  
  // Now execute the plan, unless requested not to
  if ( req.executePath )
    postStateCompletion();
  else
    postStateFailure();
}

void Pilot::ConstructNavPlan::doAnalysis(NavigationPlan &plan, Point initPos, AngTwoPi initHead, PilotRequest req) {

  AngTwoPi orientation = initHead;
  Point currentPosition = initPos;
  plan.steps.clear();

  for (unsigned int i = 1; i<(plan.path.size()); i++) {

    // Calculate vector between this and next point on path
    const Point delta = (plan.path[i] - plan.path[i-1]);
    const float distanceBetween = delta.xyNorm();
    const AngSignPi headingChange = AngSignPi(delta.atanYX() - orientation);
    orientation += headingChange;
    
    // If this step involves a non-negligible heading change, turn  first
    if( fabs(headingChange) > M_PI/18 )
      plan.addNavigationStep(turnStep, plan.path[i], orientation);
    // If we have a pan/tilt and we made a large turn, relocalize
#ifdef TGT_HAS_HEAD
    if ( fabs(headingChange) > M_PI / 6 )
      if ( req.landmarks.size() > 0)
        plan.addNavigationStep(localizeStep, plan.path[i], orientation);
#endif

    // Calculate the number of times to localize during travel, but
    // don't try to localize if we have no landmarks.
    float maxDistanceBetween = 500;   //!< farthest we're willing to travel before localizing again
    if ( req.landmarks.empty() ) {
      maxDistanceBetween = 1e10f;
    }
    int numberOfLocalizations = (int)floor(distanceBetween / maxDistanceBetween);

    Point deltaStep = delta.unitVector() * maxDistanceBetween;
    Point currentPoint = plan.path[i-1];
    for ( int j=0; j < numberOfLocalizations; j++ ) {
      currentPoint += deltaStep;
      plan.addNavigationStep(travelStep, currentPoint, orientation);
      plan.addNavigationStep(localizeStep, currentPoint, orientation);
    }
    plan.addNavigationStep(travelStep, plan.path[i], orientation);
  }
  plan.currentStep = plan.steps.begin();
  postStateCompletion();
}

void Pilot::ExecutePlan::ExecuteStep::doExecute(NavigationPlan &plan, 
                                                DisplacementInstruction &nextDisplacementInstruction,
                                                const bool pushType) {

  DualCoding::Point position = theAgent->getCentroid();

  if (plan.steps.size() == 0) {
    std::cout << "Warning: empty path in Pilot::ExecutePlan::doStart" << std::endl;
    postStateCompletion();
    return;
  }

  switch (plan.currentStep->type) {

  case localizeStep:
    if (!pushType)
        postStateSignal<NavigationStepType_t>(localizeStep);
    else
        postStateSuccess();
    break;

  case travelStep: {
    position = theAgent->getCentroid();
    fmat::Column<3> pos = position.getCoords();
    fmat::Column<3> next = plan.currentStep->waypoint.getCoords();      
    if ( verbosity & PVnavStep )
      cout << ">>> Travel: at " << pos << " hdg " << float(AngSignPi(theAgent->getOrientation()))*180/M_PI 
     << " deg., next pos. is " << next;
    if (position.xyDistanceFrom(plan.currentStep->waypoint) < allowableDistanceError) {
      if ( verbosity & PVnavStep )
  cout << ".   Close enough!" << endl;
      postStateSuccess();
    }
    else {
      fmat::Column<2> disp = fmat::SubVector<2>(next) - fmat::SubVector<2>(pos);
      nextDisplacementInstruction.nextPoint = disp;
      if ( verbosity & PVnavStep )
  cout << ", disp=" << disp << endl;

      std::vector<NavigationStep>::const_iterator m, n;
      m = plan.steps.begin();
      n = m;
      n++;
      if (((m == plan.currentStep) || (n == plan.currentStep)) && pushType)
          postStateSignal<NavigationStepType_t>(acquireObjStep);
      else
          postStateSignal<NavigationStepType_t>(travelStep);
    }
    break;
  }

  case turnStep: {
    position = theAgent->getCentroid();
    fmat::Column<3> pos = position.getCoords();
    fmat::Column<3> next = plan.currentStep->waypoint.getCoords();
    fmat::Column<2> disp = fmat::SubVector<2>(next) - fmat::SubVector<2>(pos);
    AngSignPi angle = (atan2(disp[1], disp[0]) - theAgent->getOrientation());
    if (fabs(angle) < allowableAngularError) {
      if ( verbosity & PVnavStep )
        cout << ">>> Turn: " << float(angle)*180/M_PI << " deg is smaller than minimum "
       << float(allowableAngularError)*180/M_PI << " deg (skipped)" << endl;
      postStateSuccess();
    } else {
      if ( verbosity & PVnavStep )
        cout << ">>> Turn: " << float(angle)*180/M_PI << " deg" << endl;
      nextDisplacementInstruction.angleToTurn = angle;
      if (pushType && (plan.currentStep != plan.steps.begin()))
          postStateSignal<NavigationStepType_t>(turnObjStep);
      else
          postStateSignal<NavigationStepType_t>(turnStep);
    }
    break;
    }

  default:
    cout << "Incorrect step type " << plan.currentStep->type << "in ExecuteStep" << endl;
  }
}

//================ LocalizationUtility ================

const float Pilot::LocalizationUtility::Localize::minConfidentWeight = -30;

#ifdef TGT_HAS_HEAD
void Pilot::LocalizationUtility::ChooseGazePoints::setupGazePoints(std::deque<Point> &gazePoints) {
  std::vector<ShapeRoot> visibleLandmarks =
    calculateVisibleLandmarks(theAgent->getCentroid(), theAgent->getOrientation(), 
            M_PI/2, // assume head can turn 90 degrees left or right
            VRmixin::pilot->curReq->landmarks);
  gazePoints = std::deque<Point>();
  for ( unsigned int i = 0; i < visibleLandmarks.size(); i++ ) {
    // convert to egocentric coordinates
    Point allovec = visibleLandmarks[i]->getCentroid() - theAgent->getCentroid();
    Point egovec = Point(fmat::rotationZ(- theAgent->getOrientation()) * allovec.getCoords());
    egovec.setRefFrameType(egocentric);
    gazePoints.push_back(egovec);
  }
}

#else  // robot has no head, so plan on turning the body
void Pilot::LocalizationUtility::ChooseGazePoints::setupGazeHeadings
                (const AngTwoPi initialHeading, std::deque<AngTwoPi> &gazeHeadings, const AngTwoPi maxTurn) {
  const AngTwoPi robotHeading = theAgent->getOrientation();
  std::vector<ShapeRoot> visibleLandmarks =
    calculateVisibleLandmarks(theAgent->getCentroid(), robotHeading, 
            maxTurn, // assume body can turn the full +/- 180 degrees
            VRmixin::pilot->curReq->landmarks);
  gazeHeadings = std::deque<AngTwoPi>();
  for ( unsigned int i = 0; i < visibleLandmarks.size(); i++ ) {
    Point allovec = visibleLandmarks[i]->getCentroid() - theAgent->getCentroid();
    gazeHeadings.push_back(AngTwoPi(allovec.atanYX()));
  }
  // Landmark headings are allocentric, but to minimize turn distance
  // we want to sort them by their bearing relative to the robot.
  std::sort(gazeHeadings.begin(), gazeHeadings.end(), BearingLessThan(robotHeading));
  // Reverse the list if the closest landmark is to the right instead of the left
  if ( gazeHeadings.size() > 1 &&
       abs(AngSignPi(gazeHeadings.front()-robotHeading)) > abs(AngSignPi(gazeHeadings.back()-robotHeading)) )
    reverse(gazeHeadings.begin(), gazeHeadings.end());
}

void Pilot::LocalizationUtility::PrepareForNextGazePoint::prepareForNext(std::deque<AngTwoPi> &gazeHeadings, const AngTwoPi maxTurn) {
  if ( gazeHeadings.empty() ) {
    postStateFailure();
    return;
  }
  const AngTwoPi curHeading = VRmixin::theAgent->getOrientation();
  const AngTwoPi newHeading = gazeHeadings.front();
  gazeHeadings.pop_front();
  const AngSignPi turnAmount = AngSignPi(newHeading - curHeading);
  // std::cout << "*** curHeading=" << curHeading << "  newHeading=" << newHeading
  //          << "  turnAmount=" << turnAmount << "  maxturn=" << maxTurn << std::endl;
  if (fabs(turnAmount) > maxTurn) {
    postStateSuccess();
    return;
  }
  MMAccessor<WaypointWalkMC> wp_acc(getMC());
  wp_acc->getWaypointList().clear();
#ifdef TGT_IS_CREATE
  float const va = 1.0f;  // optimal turn speed for CREATE odometry
  wp_acc->addEgocentricWaypoint(0, 0, turnAmount, true, 0, va);
#else
  // no other headless robots supported yet, so just use Create values for now
  float const va = 1.0f;
  wp_acc->addEgocentricWaypoint(0, 0, turnAmount, true, 0, va);
#endif // TGT_IS_CREATE
  motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
}

#endif // TGT_HAS_HEAD

void Pilot::LocalizationUtility::Localize::doStart() {
  VRmixin::particleFilter->updateSensors(particleFilter->getSensorModel(),false,true);
  const ShapeBasedParticleFilter::particle_type &estimate = VRmixin::particleFilter->getEstimate();
  if ( estimate.weight < minConfidentWeight ) {
    std::cout << "  Particle filter bestWeight=" << estimate.weight
              << " < minConfidentWeight=" << minConfidentWeight << ": resample." << std::endl;
    VRmixin::particleFilter->updateSensors(particleFilter->getSensorModel(),false,true);
  }
  if ( estimate.weight < minConfidentWeight ) {
    std::cout << "  Particle filter bestWeight=" << estimate.weight
              << " < minConfidentWeight=" << minConfidentWeight << ": resample once more." << std::endl;
    VRmixin::particleFilter->updateSensors(particleFilter->getSensorModel(),false,true);
  }
  AngTwoPi heading = theAgent->getOrientation();
  cout << "Agent now at " << theAgent->getCentroid() << " hdg " << heading
       << " (= " << float(heading)*180/M_PI << " deg.)" 
       << "  std. dev. " << sqrt(((ShapeBasedParticleFilter*)particleFilter)->getVariance().x) << " mm, "
       << ((ShapeBasedParticleFilter*)particleFilter)->getVariance().y * 180 / M_PI << " deg."
       << "  bestWeight=" << estimate.weight
       << endl;
  VRmixin::mapBuilder->setAgent(Point(estimate.x,estimate.y,0,allocentric), estimate.theta, true);
  if ( VRmixin::pilot->curReq->displayIndividualParticles > 0 )
    VRmixin::particleFilter->displayIndividualParticles(VRmixin::pilot->curReq->displayIndividualParticles);
  else if ( VRmixin::pilot->curReq->displayParticles > 0 )
    VRmixin::particleFilter->displayParticles(VRmixin::pilot->curReq->displayParticles);
  else {
    GET_SHAPE(particles, GraphicsData, worldShS);
    if ( particles.isValid() )
      VRmixin::particleFilter->displayParticles(50);
  }
}

//================ WalkMachine ================

void Pilot::WalkMachine::WalkMachine_WaypointWalker::doStart() {
  PilotRequest &req = *VRmixin::pilot->curReq;
  MMAccessor<WaypointWalkMC> wp_acc(getMC());
  wp_acc->getWaypointList().clear();
#ifdef TGT_IS_CREATE
  float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 50.f;
  float va = (req.turnSpeed > 0) ? req.turnSpeed : 1.0f;
  if (req.da != 0)
    wp_acc->addEgocentricWaypoint(0, 0, req.da, true, vx, va);
  if (req.dx != 0)
    wp_acc->addEgocentricWaypoint(req.dx, 0, (req.dx >= 0) ? 0 : -(float)M_PI, true, vx, va);
#else
  // these values are for Chiara
  float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 15.f;
  float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
  float daCorrected = req.da + ((req.dx >= 0) ? 0 : -(float)M_PI);
  wp_acc->addEgocentricWaypoint(req.dx, req.dy, daCorrected, true, vx, va);
#endif
  motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
}


//================ WaypointWalkmachine ================

void Pilot::WaypointWalkMachine::WaypointWalkMachine_WaypointWalker::doStart() {
  PilotRequest &req = *VRmixin::pilot->curReq;
  MMAccessor<WaypointWalkMC> wp_acc(getMC());
  if(req.clearWaypoints)
    wp_acc->clearWaypointList();
  wp_acc->appendWaypoints(*req.waypointList);
  motman->setPriority(VRmixin::VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
}

//================ SetVelocityMachine ================

void Pilot::SetVelocityMachine::SetVelocityMachine_Walker::doStart() {
  PilotRequest &preq = *VRmixin::pilot->curReq;
  MotionManager::MC_ID wid = VRmixin::pilot->getWalk_id();
  MMAccessor<WalkMC>(wid)->setTargetVelocity(preq.forwardSpeed, preq.strafeSpeed, preq.turnSpeed);
  motman->setPriority(wid,MotionManager::kStdPriority);
}

void Pilot::cancelVelocity() {
  if ( VRmixin::pilot->curReq != NULL && VRmixin::pilot->curReq->requestType == PilotTypes::setVelocity ) {
    MotionManager::MC_ID wid = VRmixin::pilot->getWalk_id();
    erouter->postEvent(EventBase(EventBase::motmanEGID, wid, EventBase::statusETID, 0));  // force WalkNode to complete
  }
  else
    std::cout << "Warning: call to Pilot::cancelVelocity() when not executing a setVelocity request: ignored." << std::endl;
}

//================ GoToShapeMachine ================

const float Pilot::ExecutePlan::allowableDistanceError = 50;
const float Pilot::ExecutePlan::allowableAngularError = 0.15;

void Pilot::GoToShapeMachine::CheckParameters::doStart() {

  PilotRequest &req = *VRmixin::pilot->curReq;
  if ( ! req.targetShape.isValid() ) {
    cout << "Pilot request goToShape fails: no valid targetShape specified" << endl;
    postStateFailure();
    return;
  }
  if ( req.targetShape->isObstacle() ) {
    cout << "Warning: Pilot marking targetShape '" << req.targetShape->getName()
   << "' (" << req.targetShape->getId() << ") as non-obstacle to permit path planning." << endl;
    req.targetShape->setObstacle(false);
  }
  // arguments look good; okay to proceed
  postStateSuccess();
}

//================PushObjectMachine================
void Pilot::PushObjectMachine::CheckParameters::doStart() {

  PilotRequest &req = *VRmixin::pilot->curReq;
  if ( ! req.objectShape.isValid() ) {
    cout << "Pilot request pushObject fails: no valid objectShape specified" << endl;
    postStateFailure();
    return;
  if ( ! req.targetShape.isValid() ) {
    cout << "Pilot request pushObject fails: no valid targetShape specified" << endl;
    postStateFailure();
    return;
  }
  if ( req.targetShape->isObstacle() ) {
    cout << "Warning: Pilot marking targetShape '" << req.targetShape->getName()
   << "' (" << req.targetShape->getId() << ") as non-obstacle to permit path planning." << endl;
    req.targetShape->setObstacle(false);
  }
  }
  // arguments look good; okay to proceed
  postStateSuccess();
}


//=== Pilot static variables 
  unsigned int Pilot::verbosity = -1U & ~Pilot::PVnavStep;

} // namespace

#endif