C++ ArRobot::com方法代码示例

int main(int argc, char **argv)
{
  std::string str;
  int ret;
  int successes = 0, failures = 0;
  int action;
  bool exitOnFailure = true;
  
  ArSerialConnection con;
  ArRobot robot;
  //ArLog::init(ArLog::StdOut, ArLog::Verbose);
  srand(time(NULL));
  robot.runAsync(false);
// if (!exitOnFailure)
//    ArLog::init(ArLog::None, ArLog::Terse);
  //else
  //ArLog::init(ArLog::None);
  while (1)
  {
    if (con.getStatus() != ArDeviceConnection::STATUS_OPEN &&
	(ret = con.open()) != 0)
    {
      str = con.getOpenMessage(ret);
      printf("Open failed: %s\n", str.c_str());
      ++failures;
      if (exitOnFailure)
      {
	printf("Failed\n");
	exit(0);
      }
      else
      {
	ArUtil::sleep(200);
	robot.unlock();
	continue;
      }
    }
    robot.lock();
    robot.setDeviceConnection(&con);
    robot.unlock();
    ArUtil::sleep((rand() % 5) * 100);
    if (robot.asyncConnect())
    {
      robot.waitForConnectOrConnFail();
      robot.lock();
      if (!robot.isConnected())
      {
	if (exitOnFailure)
	{
	  printf("Failed after %d tries.\n", successes);
	  exit(0);
	}
	printf("Failed to connect successfully");
	++failures;
      }
      robot.comInt(ArCommands::SONAR, 0);
      robot.comInt(ArCommands::SOUNDTOG, 0);
      //robot.comInt(ArCommands::PLAYLIST, 0);
      robot.comInt(ArCommands::ENCODER, 1);
      ArUtil::sleep(((rand() % 20) + 3) * 100);
      ++successes;
      // okay, now try to leave it in a messed up state
      action = rand() % 8;
      robot.dropConnection();
      switch (action) {
      case 0:
	printf("Discon  0 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	break;
      case 1:
	printf("Discon  1 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	ArUtil::sleep(100);
	robot.com(1);
	break;
      case 2:
	printf("Discon  2 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(0);
	ArUtil::sleep(100);
	robot.com(1);
	ArUtil::sleep(100);
	robot.com(2);
	break;
      case 3:
	printf("Discon 10 ");
	robot.disconnect();
	ArUtil::sleep(100);
	robot.com(10);
	break;
      case 4:
	printf("Discon    ");
	robot.disconnect();
	break;
      default:
//.........这里部分代码省略.........

int main(int argc, char **argv)
{
  int numPowerSpecs = sizeof(PowerSpecs)/sizeof(powerspec);
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArRobot robot;
  ArRobotConnector robotConnector(&parser, &robot);

  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "seekurPower: Error: Could not connect to the robot.");
    Aria::logOptions();
    Aria::exit(1);
  }

  if (!Aria::parseArgs() || parser.checkArgument("help"))
  {
    Aria::logOptions();
    ArLog::log(ArLog::Terse, "Options for seekurPower command (your robot may not have some of these devices):");
    ArLog::log(ArLog::Terse, "-<n> <on|off|reset>\t\tTurn port <n> on or off, or reset by turning off then on again. Refer to robot documentation or http://robots.mobilerobots.com/wiki/Seekur_Switched_Power_Outputs for list and notes.");
    for(int i = 0; i < numPowerSpecs; ++i)
      ArLog::log(ArLog::Terse, "-%s <on|off|reset>\t\t%s (port %d)", PowerSpecs[i].option, PowerSpecs[i].description, PowerSpecs[i].port);
    ArLog::log(ArLog::Terse, "-robotOff\t\tTurn whole robot off");
    Aria::exit(2);
  }

  if(parser.checkArgument("-robotOff"))
  {
    ArLog::log(ArLog::Terse, "-robotOff given -- powering down entire robot with command #119 in 5 seconds...");
    for(int i = 5; i > 0; --i)
    {
      ArLog::log(ArLog::Terse, "Shutting down in %d seconds...", i);
      ArUtil::sleep(1000);
    }
    robot.com(119);
    Aria::exit(0);
  }
      

  std::list<powerspec> todo;

  int nargs = parser.getArgc();
  for(int argi = 1; argi < nargs; ++argi)
  {
    const char *opt = parser.getArg(argi);
    if(opt[0] != '-')
    {
      ArLog::log(ArLog::Terse, "seekurPower: Error: invalid option %s. Use -help for list of options.", opt);
      Aria::exit(4);
    }
    ++opt;
    if(opt[0] == '-')
      ++opt;
    bool found = false;
    if(argi == nargs-1) // option given as last argument, with no value
    {
      ArLog::log(ArLog::Terse, "seekurPower: Error: Missing argument to last option %s. Specify on, off or reset.", opt);
      Aria::exit(7);
    }
    const char *val = parser.getArg(++argi);
    for(int pi = 0; pi < numPowerSpecs; ++pi)
    {
      powerspec ps = PowerSpecs[pi];
      if(strcmp(opt, ps.option) == 0) 
      {
        found = true;
        if(strcmp(val, "on") == 0)
          ps.set = 1;
        else if(strcmp(val, "off") == 0)
          ps.set = 0;
        else if(strcmp(val, "reset") == 0)
          ps.set = 2;
        else
        {
          ArLog::log(ArLog::Terse, "seekurPower: Error: Invalid value '%s' for option %s. Use on, off or reset.", val, opt);
          Aria::exit(6);
        }
        todo.push_back(ps);
        break;
      }
    }
    if(!found)
    {
      if(ArUtil::isOnlyNumeric(opt))
      {
        found = true;
        powerspec ps;
        ps.option = opt;
        ps.description = NULL;
        ps.port = atoi(opt);
        if(strcmp(val, "on") == 0)
          ps.set = 1;
        else if(strcmp(val, "off") == 0)
          ps.set = 0;
        else if(strcmp(val, "reset") == 0)
          ps.set = 2;
        todo.push_back(ps);
      }
    }
//.........这里部分代码省略.........

//.........这里部分代码省略.........

  // Add range device which uses forbidden regions given in the map to give virtual
  // range device readings to ARNL.  (so it avoids obstacles
  // detected by this device)
  ArForbiddenRangeDevice forbidden(&map);
  robot.addRangeDevice(&forbidden);
  pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);

  robot.unlock();


  // Action to slow down robot when localization score drops but not lost.
  ArActionSlowDownWhenNotCertain actionSlowDown(&locTask);
  pathTask.getPathPlanActionGroup()->addAction(&actionSlowDown, 140);

  // Action to stop the robot when localization is "lost" (score too low)
  ArActionLost actionLostPath(&locTask, &pathTask);
  pathTask.getPathPlanActionGroup()->addAction(&actionLostPath, 150);

  // Arnl uses this object when it must replan its path because its
  // path is completely blocked.  It will use an older history of sensor
  // readings to replan this new path.  This should not be used with SONARNL
  // since sonar readings are not accurate enough and may prevent the robot
  // from planning through space that is actually clear.
  ArGlobalReplanningRangeDevice replanDev(&pathTask);

  
  // Service to provide drawings of data in the map display :
  ArServerInfoDrawings drawings(&server);
  drawings.addRobotsRangeDevices(&robot);
  drawings.addRangeDevice(&replanDev);

  /* Draw a box around the local path planning area use this 
    (You can enable this particular drawing from custom commands 
    which is set up down below in ArServerInfoPath) */
  ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75);
  ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *> 
    drawingFunctorP(&pathTask, &ArPathPlanningTask::drawSearchRectangle);
  drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP); 

  /* Show the sample points used by MCL */
  ArDrawingData drawingDataL("polyDots", ArColor(0,255,0), 100, 75);
  ArFunctor2C<ArLocalizationTask, ArServerClient *, ArNetPacket *> 
    drawingFunctorL(&locTask, &ArLocalizationTask::drawRangePoints);
  drawings.addDrawing(&drawingDataL, "Localization Points", &drawingFunctorL);


  // "Custom" commands. You can add your own custom commands here, they will
  // be available in MobileEyes' custom commands (enable in the toolbar or
  // access through Robot Tools)
  ArServerHandlerCommands commands(&server);


  // These provide various kinds of information to the client:
  ArServerInfoRobot serverInfoRobot(&server, &robot);
  ArServerInfoSensor serverInfoSensor(&server, &robot);
  ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
  serverInfoPath.addSearchRectangleDrawing(&drawings);
  serverInfoPath.addControlCommands(&commands);

  // Provides localization info and allows the client (MobileEyes) to relocalize at a given
  // pose:
  ArServerInfoLocalization serverInfoLocalization(&server, &robot, &locTask);
  ArServerHandlerLocalization serverLocHandler(&server, &robot, &locTask);

  // If you're using MobileSim, ArServerHandlerLocalization sends it a command