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

int main(int argc, char** argv)
{
  // set up our simpleConnector
  ArSimpleConnector simpleConnector(&argc, argv);
  // robot
  ArRobot robot;
  // a key handler so we can do our key handling
  ArKeyHandler keyHandler;

  ArLog::init(ArLog::StdOut,ArLog::Verbose);

  // if there are more arguments left then it means we didn't
  // understand an option
  if (!simpleConnector.parseArgs() || argc > 1)
  {    
    simpleConnector.logOptions();
    keyHandler.restore();
    exit(1);
  }

  // mandatory init
  Aria::init();
  ArLog::init(ArLog::StdOut, ArLog::Terse, NULL, true);

  // let the global aria stuff know about it
  Aria::setKeyHandler(&keyHandler);
  // toss it on the robot
  robot.attachKeyHandler(&keyHandler);

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

  // turn on the motors for the velocity response test
  robot.comInt(ArCommands::ENABLE, 1);
  velTime.setToNow();

  // turn off the sonar
  robot.comInt(ArCommands::SONAR, 0);

  ArGlobalFunctor1<ArRobot *> userTaskCB(&userTask, &robot);
  robot.addUserTask("iotest", 100, &userTaskCB);

  robot.comInt(ArCommands::IOREQUEST, 1);
  requestTime.setToNow();

  //start the robot running, true so that if we lose connection the run stops
  robot.run(true);
  
  // now exit
  Aria::shutdown();
  return 0;


}

int main()
{
  ArSyncTask *task;
  UserTaskTest test;
  ArRobot robot;

  ArFunctorC<UserTaskTest> userTaskOne(&test, &UserTaskTest::userTaskOne);
  ArFunctorC<UserTaskTest> userTaskTwo(&test, &UserTaskTest::userTaskTwo);
  ArFunctorC<UserTaskTest> userTaskThree(&test, &UserTaskTest::userTaskThree);
  ArFunctorC<UserTaskTest> userTaskFour(&test, &UserTaskTest::userTaskFour);


  printf("Before tasks added:\n");
  robot.logUserTasks();
  printf("---------------------------------------------------------------\n");

  // the order you add tasks doesn't matter, it goes off of the integer
  // you give the function call to add them 
  // Caveat: if you give the function call the same number it goes off of order
  robot.addUserTask("procTwo", 20, &userTaskTwo);
  robot.addUserTask("procFour", 40, &userTaskFour, &test.myTaskFourState);
  robot.addUserTask("procThree", 30, &userTaskThree);
  robot.addUserTask("procOne", 10, &userTaskOne);

  printf("After tasks added:\n");
  robot.logUserTasks();
  printf("---------------------------------------------------------------\n");
  printf("What happens when these are run:\n");
  robot.loopOnce();

  printf("---------------------------------------------------------------\n");

  printf("After tasks removed by name:\n");
  printf("---------------------------------------------------------------\n");
  robot.remUserTask("procOne");
  robot.remUserTask("procTwo");
  robot.remUserTask("procThree");
  robot.remUserTask("procFour");
  robot.logUserTasks();


  printf("\nAfter they are added again, procThree is found by name and set to SUSPEND and procFour is found by functor and set to FAILURE:\n");
  printf("---------------------------------------------------------------\n");

  printf("---------------------------------------------------------------\n");
  robot.addUserTask("procTwo", 20, &userTaskTwo);
  robot.addUserTask("procFour", 40, &userTaskFour, &test.myTaskFourState);
  robot.addUserTask("procThree", 30, &userTaskThree);
  robot.addUserTask("procOne", 10, &userTaskOne);
  task = robot.findUserTask("procThree");
  if (task != NULL)
    task->setState(ArTaskState::SUSPEND);

  task = robot.findUserTask(&userTaskFour);
  if (task != NULL) 
    task->setState(ArTaskState::FAILURE);

  robot.logUserTasks();

  task = robot.findUserTask(&userTaskFour);
  if (task != NULL)
  {
    printf("---------------------------------------------------------------\n");

    printf("Did the real state on procFourState get set:\n");
    printf("getState: %d realState: %d\n", task->getState(), 
	   test.myTaskFourState);
  }


  printf("---------------------------------------------------------------\n");
  printf("What happens when these are run:\n");
  robot.loopOnce();

  printf("---------------------------------------------------------------\n");


  printf("After tasks removed by functor:\n");
  printf("---------------------------------------------------------------\n");
  robot.remUserTask(&userTaskOne);
  robot.remUserTask(&userTaskTwo);
  robot.remUserTask(&userTaskThree);
  robot.remUserTask(&userTaskFour);
  robot.logUserTasks();
  
  
}

int main(int argc, char **argv)
{
  // robot
  ArRobot robot;
  // the laser
  ArSick sick;


  // sonar, must be added to the robot
  //ArSonarDevice sonar;

  // the actions we'll use to wander
  // recover from stalls
  //ArActionStallRecover recover;
  // react to bumpers
  //ArActionBumpers bumpers;
  // limiter for close obstacles
  ArActionLimiterForwards limiter("speed limiter near", 1600, 0, 0, 1.3);
  // limiter for far away obstacles
  //ArActionLimiterForwards limiterFar("speed limiter near", 300, 1000, 450, 1.1);
  //ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 600, 1.1);
  // limiter for the table sensors
  //ArActionLimiterTableSensor tableLimiter;
  // actually move the robot
  ArActionConstantVelocity constantVelocity("Constant Velocity", 1500);
  // turn the orbot if its slowed down
  ArActionTurn turn;

  // mandatory init
  Aria::init();

  // Parse all our args
  ArSimpleConnector connector(&argc, argv);
  if (!connector.parseArgs() || argc > 1)
  {
    connector.logOptions();
    exit(1);
  }
  
  // add the sonar to the robot
  //robot.addRangeDevice(&sonar);
  // add the laser to the robot
  robot.addRangeDevice(&sick);

  // try to connect, if we fail exit
  if (!connector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.comInt(ArCommands::SONAR, 0);

  // turn on the motors, turn off amigobot sounds
  //robot.comInt(ArCommands::SONAR, 0);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  // add the actions
  //robot.addAction(&recover, 100);
  //robot.addAction(&bumpers, 75);
  robot.addAction(&limiter, 49);
  //robot.addAction(&limiter, 48);
  //robot.addAction(&tableLimiter, 50);
  robot.addAction(&turn, 30);
  robot.addAction(&constantVelocity, 20);

  robot.setStateReflectionRefreshTime(50);
  limiter.activate();
  turn.activate();
  constantVelocity.activate();

  robot.clearDirectMotion();
  //robot.setStateReflectionRefreshTime(50);
  robot.setRotVelMax(50);
  robot.setTransAccel(1500);
  robot.setTransDecel(100);

  // start the robot running, true so that if we lose connection the run stops
  robot.runAsync(true);

  connector.setupLaser(&sick);

  // now that we're connected to the robot, connect to the laser
  sick.runAsync();

  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    Aria::shutdown();
    return 1;
  }
  
  sick.lockDevice();
  sick.setMinRange(250);
  sick.unlockDevice();
  robot.lock();
  ArGlobalFunctor1<ArRobot *> userTaskCB(&userTask, &robot);
  robot.addUserTask("iotest", 100, &userTaskCB);
  requestTime.setToNow();
//.........这里部分代码省略.........