advanced.cpp

sonarnl localization and path planning by mobile robots

    {
      myGoalList.push_back(obj);
      ArPose pose = obj->getPose();
      printf("GoalWithHeading[%d] = %s : %5.2f %5.2f %5.2f\n", 
	     i++, obj->getName(),
	     pose.getX(), pose.getY(), pose.getTh());
    }
    if (strcasecmp(obj->getType(), "Goal") == 0)
    {
      myGoalList.push_back(obj);
      ArPose pose = obj->getPose();
      printf("Goal[%d] = %s : %5.2f %5.2f %5.2f\n", i++, obj->getName(),
	     pose.getX(), pose.getY(), pose.getTh());
    }
  }
}
/*!
 * Gets the list of home poses.
 *
 * @param mapdata: Pointer to the map class.
 *
 * @return list of home objects.
 *
 */
void
Advanced::getAllRobotHomes(ArMap* ariamap)
{
  myHomeList.clear();
  std::list<ArMapObject *>::iterator objIt;
  ArMapObject* obj;
  int i=0;
  for (objIt = ariamap->getMapObjects()->begin(); 
       objIt != ariamap->getMapObjects()->end(); 
       objIt++)
  {
    //
    // Get the forbidden lines and fill the occupancy grid there.
    //
    obj = (*objIt);
    if (strcasecmp(obj->getType(), "RobotHome") == 0)
    {
      myHomeList.push_back(obj);
      ArPose pose = obj->getPose();
      printf("RobotHome[%d] = %s : %5.2f %5.2f %5.2f\n", i++, obj->getName(),
	     pose.getX(), pose.getY(), pose.getTh());
    }
    if (strcasecmp(obj->getType(), "Home") == 0)
    {
      myHomeList.push_back(obj);
      ArPose pose = obj->getPose();
      printf("Home[%d] = %s : %5.2f %5.2f %5.2f\n", i++, obj->getName(),
	     pose.getX(), pose.getY(), pose.getTh());
    }
  }
}
/*!
 * Callback function for the l key. Localizes the robot at the home position.
 * Warning: Robot must be physically at the first home pose.
 */
void 
lkeyCB(void)
{
  roundRobinFlag = false;

  static int once = 0;
  ArPose pose;
  if(!once && advancedptr->myHomeList.size()>0){
    once = 1;
    ArPose top = advancedptr->myHomeList.front()->getPose();
    printf("Localizing at %s %5.2f %5.2f %5.2f\n", 
	   advancedptr->myHomeList.front()->getName(),
	   top.getX(), top.getY(), top.getTh());
    //
    // Set the pose to localize the robot about.
    //
    pose = top;
  } else {
    advancedptr->myRobot->lock();
    pose = advancedptr->myRobot->getPose();
    advancedptr->myRobot->unlock();
  }
  //
  // Do the localization at the current pose.
  //
  if(advancedptr->localizeAtSetPose(pose)){
    printf("Localized at current pose: Score: %5.2f out of 1.0\n",
	   advancedptr->getLocalizationScore());
  }else {
    printf("Failed to localize\n");
  }
}
/*!
 * Callback function for the p key. Gets the robot to pick the next
 * goal point and plan to it.
 */
void 
pkeyCB(void)
{
  roundRobinFlag = false;

  if(advancedptr->myLocaTask->getInitializedFlag()){
    if(advancedptr->myGoalList.size()>0){
      ArMapObject* front = advancedptr->myGoalList.front();
      ArPose top = front->getPose();
      bool headingFlag = false;
      if(strcasecmp(front->getType(), "GoalWithHeading") == 0)
	headingFlag = true;
      else
	headingFlag = false;
      printf("Path planing to goal %s %5.2f %5.2f %5.2f\n", 
	     front->getName(),
	     top.getX(), top.getY(), top.getTh());
      advancedptr->myGoalList.pop_front();
      advancedptr->myGoalList.push_back(front);
      //
      // Setup the pathplanning task to this destination.
      //
      advancedptr->pathPlanFromCurrentToDest(top, headingFlag);
    }
  }else{
    printf("Localize the robot first\n");
  }
}
/*!
 * Callback function for the r key. Gets the robot to pick the next
 * goal point and plan to it. and keep going around the list.
 */
void 
rkeyCB(void)
{
  if(roundRobinFlag == false){
    roundRobinFlag = true;
  }else{
    roundRobinFlag = false;
    return;
  }
  
  if(advancedptr->myLocaTask->getInitializedFlag()){
    if(advancedptr->myGoalList.size()>0){
      ArMapObject* front = advancedptr->myGoalList.front();
      ArPose top = front->getPose();
      bool headingFlag = false;
      if(strcasecmp(front->getType(), "GoalWithHeading") == 0)
	headingFlag = true;
      else
	headingFlag = false;
      printf("Path planing to goal %s %5.2f %5.2f %5.2f: %d poses\n", 
	     front->getName(),
	     top.getX(), top.getY(), top.getTh(), 
	     advancedptr->myGoalList.size());
      advancedptr->myGoalList.pop_front();
      advancedptr->myGoalList.push_back(front);
      //
      // Setup the pathplanning task to this destination.
      //
      advancedptr->pathPlanFromCurrentToDest(top, headingFlag);
    }
  }else{
    printf("Localize the robot first\n");
  }
}
/*!
 * Callback function for the h key. Gets the robot to home pose.
 */
void 
hkeyCB(void)
{
  roundRobinFlag = false;

  if(advancedptr->myLocaTask->getInitializedFlag()){
    if(advancedptr->myHomeList.size()>0){
      ArMapObject* front = advancedptr->myHomeList.front();
      ArPose top = front->getPose();
      printf("Homing to %s %5.2f %5.2f %5.2f\n", 
	     front->getName(),
	     top.getX(), top.getY(), top.getTh());
      bool headingFlag = true;
      advancedptr->pathPlanFromCurrentToDest(top, headingFlag);
    }
  }else{
    printf("Localize the robot first\n");
  }
}
/*!
 * Callback function for the q key.
 */
void 
quitCB(void)
{
  roundRobinFlag = false;
  keyHandler.restore();
  advancedptr->shutDown();  
}
/*!
 * Interact with user on the terminal.
 */
void
interact()
{
  ArMap* ariamap = advancedptr->myMap;
  sleep(1);
  advancedptr->getAllGoals(ariamap);
  advancedptr->getAllRobotHomes(ariamap);

  /// MPL
//  lkeyCB();
  advancedptr->myLocaTask->localizeRobotAtHomeNonBlocking();
  //
  // Interact with user using keyboard.
  //
  ArGlobalFunctor lCB(&lkeyCB);
  ArGlobalFunctor pCB(&pkeyCB);
  ArGlobalFunctor hCB(&hkeyCB);
  ArGlobalFunctor rCB(&rkeyCB);
  ArGlobalFunctor qCB(&quitCB);
  ArGlobalFunctor escapeCB(&quitCB);

  keyHandler.addKeyHandler('l', &lCB);
  keyHandler.addKeyHandler('p', &pCB);
  keyHandler.addKeyHandler('h', &hCB);
  keyHandler.addKeyHandler('r', &rCB);
  keyHandler.addKeyHandler('q', &qCB);
  keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);

  printf("Put robot at RobotHome and press 'l' to localize first.\n\
Press 'p' to move to the next goal\n\
Press 'h' to move to the first home\n\
Press 'r' to move to the goals in order\n\
Press 'q' to quit\n");
  while (advancedptr->myLocaTask->getRunning() &&
	 advancedptr->myPathPlanningTask->getRunning()){

    keyHandler.checkKeys();
    ArUtil::sleep(250);

    advancedptr->myRobot->lock();
    ArPose rpose = advancedptr->myRobot->getPose();
    double lvel = advancedptr->myRobot->getVel();
    double avel = advancedptr->myRobot->getRotVel();
    double volts = advancedptr->myRobot->getBatteryVoltage();
    advancedptr->myRobot->unlock();
    if(advancedptr->myLocaTask->getInitializedFlag()){
      printf("\r%5.2f %5.2f %5.2f: %5.2f %5.2f: %4.1f\r", 
	     rpose.getX(), rpose.getY(), rpose.getTh(), lvel, avel, volts);
      fflush(stdout);
    }
  }
}
/*!
 * Main function which initializes the localization tasks and pathplanning
 * tasks.
 *
 * @param argc No of command line args.
 * @param argv Array of command line args.
 *
 * @return 1 if successful.
 */
int 
main(int argc, char *argv[])
{
  char* mapname;

  if(argc<2){
    printf("%s\n",USAGE);
    exit(1);
  }

  Aria::init();
  Arnl::init();

  // The robot object
  ArRobot robot;
#ifndef SONARNL
  // The laser object
  ArSick sick(181, 361);
#endif


  // Set up our simpleConnector
  ArSimpleConnector simpleConnector(&argc, argv);

  // Parse its arguments
  simpleConnector.parseArgs();

  // Parse them command line arguments.
  ArArgumentParser parser(&argc, argv);

  // Sonar, must be added to the robot, for teleop and wander
  ArSonarDevice sonarDev;
  // Add the sonar to the robot
  robot.addRangeDevice(&sonarDev);

  // Set up the robot for connecting
  if (!simpleConnector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.enableMotors();
  robot.clearDirectMotion();

#ifndef SONARNL
  // Set up the laser before handing it to the laser mode
  simpleConnector.setupLaser(&sick);

  // Add the laser to the robot
  robot.addRangeDevice(&sick);
#endif

  // Start the robot thread.
  robot.runAsync(true);

#ifndef SONARNL
  // Start the laser thread.
  sick.runAsync();

  // Connect the laser
  if (!sick.blockingConnect())
  {
    printf("Couldn't connect to sick, exiting\n");
    Aria::shutdown();
    return 1;
  }
#endif

  //
  // Set up the big holder.
  //
#ifndef SONARNL
  advancedptr = new Advanced(&robot, &sick, &sonarDev);
#else
  advancedptr = new Advanced(&robot, NULL, &sonarDev);
#endif
  //
  // Set up the callbacks for localization task.
  //
  ArFunctor1C<Advanced, int> 
  failed(advancedptr, &Advanced::trackingFailed);
  //
  // Set up the callbacks for pathplanning task.
  //
  ArFunctor1C<Advanced, ArPose> 
  goal_done(advancedptr, &Advanced::goalDone);
  ArFunctor1C<Advanced, ArPose> 
  goal_failed(advancedptr, &Advanced::goalFailed);

  // Read in config file. Do this before creating the localization
  // and path planning tasks, so we can override certain config
  // parameters such as the map file name when we do so.
  if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
  {
    printf("Trouble loading configuration file, exiting\n");
    exit(1);
  }

  //
  // Get mapname and initialize localization task and pathplanning tasks.
  //
  if((mapname = parser.checkParameterArgument("-map"))){
    //
    // Start the localization thread. Should be done first.
    //
    if(!advancedptr->initializeLocalizationTask(mapname)){
      printf("Cannot start the localization task thread.\n");
      exit(1);
    }else{
      printf("Started the Localization Thread\n");
    }
    //
    // Get the Aria map from the localization task .
    // (Alternatively use a ArMap* made separately)
    //
    advancedptr->myMap = advancedptr->myLocaTask->getAriaMap();
    //
    // Set up the path plan structure.
    // Cannot precede localization task setup.
    //
    if(!advancedptr->initializePathPlanningTask()){
      printf("Cannot set up for path planning task thread.\n");
      exit(1);
    }else{
      printf("Started Path Planning Thread\n");
    }
    //
    // Functors to be used by the Arnl library if the threads
    // signal failure or success.
    //
    advancedptr->myLocaTask->addFailedLocalizationCB(&failed);
    advancedptr->myPathPlanningTask->addGoalDoneCB(&goal_done);
    advancedptr->myPathPlanningTask->addGoalFailedCB(&goal_failed);

  }else{
    printf(USAGE);
    exit(1);
  }

  //
  // Get user input.
  //
  interact();

}