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

int main(void)
{
  ArTcpConnection con;
  ArRobot robot;
  int ret;
  std::string str;
  JoydriveAction jdAct;
  FillerThread ft;

  ft.create();

  FillerThread ft2;

  ft2.create();

  Aria::init();
  /*
  if (!jdAct.joystickInited())
  {
    printf("Do not have a joystick, set up the joystick then rerun the program\n\n");
    Aria::shutdown();
    return 1;
  }
  */
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }
  
  robot.setDeviceConnection(&con);
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

  robot.comInt(ArCommands::SONAR, 0);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.comInt(ArCommands::SOUNDTOG, 0);

  lastLoopTime.setToNow();
  loopTime = robot.getCycleTime();

  robot.addAction(&jdAct, 100);
  robot.runAsync(true);
  
  robot.waitForRunExit();
  Aria::shutdown();
  return 0;
}

int main(int argc, char **argv) 
{
  std::string str;
  int ret;

  // connection to the robot
  ArTcpConnection con;
  // the robot
  ArRobot robot;
  
  // ake the joydrive object, which also creates its own thread
  Joydrive joyd(&robot);
  
  // the connection handler
  ConnHandler ch(&robot, &joyd);

  // init aria, which will make a dedicated signal handling thread
  Aria::init(Aria::SIGHANDLE_THREAD);

  // open the connection with default args, exit if it fails
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }


  // set the connection on the robot
  robot.setDeviceConnection(&con);

  // run the robot in its own thread
  robot.runAsync(false);
  
  // have the robot connect asyncronously (so its loop is still running)
  // if this fails it means that the robot isn't running in its own thread
  if (!robot.asyncConnect())
  {
    printf(
    "asyncConnect failed because robot is not running in its own thread.\n");
    Aria::shutdown();
    return 1;
  }

  // now we just wait for the robot to be done running
  printf("Waiting for the robot's run to exit.\n");
  robot.waitForRunExit();
  // then we exit
  printf("exiting main\n");
  Aria::exit(0);  // exit program
  return 0;
}

int main(int argc, char **argv) 
{
    Aria::init();
    ArArgumentParser parser(&argc, argv);
    parser.loadDefaultArguments();
    ArRobot robot;
    ArRobotConnector robotConnector(&parser, &robot);
    if(!robotConnector.connectRobot())
    {
      ArLog::log(ArLog::Terse, "dpptuExample: Could not connect to the robot.");
      if(parser.checkHelpAndWarnUnparsed())
      {
          Aria::logOptions();
          Aria::exit(1);
      }
    }
    if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
    {
      Aria::logOptions();
      Aria::exit(1);
    }
    ArLog::log(ArLog::Normal, "dpptuExample: Connected to robot.");

  robot.runAsync(true);

  // an object for keyboard control, class defined above, this also adds itself as a user task
  KeyPTU ptu(&robot);


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

  printf("Press '?' for available commands\r\n");

  // run, if we lose connection to the robot, exit
  robot.waitForRunExit();

  Aria::exit(0);
}

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


    /* Load configuration values, map, and begin! */

  
  // When parsing the configuration file, also look at the program's command line options 
  // from the command-line argument parser as well as the configuration file.
  // (So you can use any argument on the command line, namely -map.) 
  Aria::getConfig()->useArgumentParser(&parser);
  puts("xxx");puts("aaa"); fflush(stdout);
  // Read in parameter files.
  ArLog::log(ArLog::Normal, "Loading config file %s into ArConfig (base directory %s)...", Arnl::getTypicalParamFileName(), Aria::getConfig()->getBaseDirectory());
  if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
  {
    ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
    Aria::exit(5);
  }

  // Warn about unknown params.
  if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
  {
    logOptions(argv[0]);
    Aria::exit(6);
  }

  // Warn if there is no map
  if (map.getFileName() == NULL || strlen(map.getFileName()) <= 0)
  {
    ArLog::log(ArLog::Normal, "");
    ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure");
    ArLog::log(ArLog::Normal, "   0) You can find this information in README.txt or docs/Mapping.txt");
    ArLog::log(ArLog::Normal, "   1) Connect to this server with MobileEyes");
    ArLog::log(ArLog::Normal, "   2) Go to Tools->Map Creation->Start Scan");
    ArLog::log(ArLog::Normal, "   3) Give the map a name and hit okay");
    ArLog::log(ArLog::Normal, "   4) Drive the robot around your space (see docs/Mapping.txt");
    ArLog::log(ArLog::Normal, "   5) Go to Tools->Map Creation->Stop Scan");
    ArLog::log(ArLog::Normal, "   6) Start up Mapper3");
    ArLog::log(ArLog::Normal, "   7) Go to File->Open on Robot");
    ArLog::log(ArLog::Normal, "   8) Select the .2d you created");
    ArLog::log(ArLog::Normal, "   9) Create a .map");
    ArLog::log(ArLog::Normal, "  10) Go to File->Save on Robot");
    ArLog::log(ArLog::Normal, "  11) In MobileEyes, go to Tools->Robot Config");
    ArLog::log(ArLog::Normal, "  12) Choose the Files section");
    ArLog::log(ArLog::Normal, "  13) Enter the path and name of your new .map file for the value of the Map parameter.");
    ArLog::log(ArLog::Normal, "  14) Press OK and your new map should become the map used");
    ArLog::log(ArLog::Normal, "");    
  }

  // Print a log message notifying user of the directory for map files
  ArLog::log(ArLog::Normal, "");
  ArLog::log(ArLog::Normal, 
	     "Directory for maps and file serving: %s", fileDir);
  
  ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
  ArLog::log(ArLog::Normal, "");

  // Do an initial localization of the robot. It tries all the home points
  // in the map, as well as the robot's current odometric position, as possible
  // places the robot is likely to be at startup.   If successful, it will
  // also save the position it found to be the best localized position as the
  // "Home" position, which can be obtained from the localization task (and is
  // used by the "Go to home" network request).
  locTask.localizeRobotAtHomeBlocking();
  
  // Let the client switch manager (for multirobot) spin off into its own thread
  // TODO move to multirobot example?
  clientSwitch.runAsync();

  // Start the networking server's thread
  server.runAsync();


  // Add a key handler so that you can exit by pressing
  // escape. Note that this key handler, however, prevents this program from
  // running in the background (e.g. as a system daemon or run from 
  // the shell with "&") -- it will lock up trying to read the keys; 
  // remove this if you wish to be able to run this program in the background.
  ArKeyHandler *keyHandler;
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    keyHandler = new ArKeyHandler;
    Aria::setKeyHandler(keyHandler);
    robot.lock();
    robot.attachKeyHandler(keyHandler);
    robot.unlock();
    puts("Server running. To exit, press escape.");
  }


 	
   ArnlASyncTaskExample asyncTaskExample(&pathTask, &robot, &modeGoto, &parser);



  // Enable the motors and wait until the robot exits (disconnection, etc.) or this program is
  // canceled.
  robot.enableMotors();
  robot.waitForRunExit();
  Aria::exit(0);
}

//.........这里部分代码省略.........
	time_t timer;
	char buffer[120];
	//for loggin
	ArRobotConnector robotConnector(&argParser, &robot);
	ArLaserConnector laserConnector(&argParser, &robot, &robotConnector);
	int32_t count = 0;
	readyLog(stream);
	if (!robotConnector.connectRobot())
	{
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if (argParser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}


	// Trigger argument parsing
	if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
	{
		Aria::logOptions();
		Aria::exit(1);
		return 1;
	}

	ArKeyHandler keyHandler;
	Aria::setKeyHandler(&keyHandler);
	robot.attachKeyHandler(&keyHandler);

	puts("This program will make the robot wander around. It uses some avoidance\n"
		"actions if obstacles are detected, otherwise it just has a\n"
		"constant forward velocity.\n\nPress CTRL-C or Escape to exit.");

	//ArSonarDevice sonar;
	//robot.addRangeDevice(&sonar);
	robot.addRangeDevice(&laser);

	robot.runAsync(true);



	// try to connect to laser. if fail, warn but continue, using sonar only
	if (!laserConnector.connectLasers())
	{
		ArLog::log(ArLog::Normal, "Warning: unable to connect to requested lasers, will wander using robot sonar only.");
	}

	double sampleAngle, dist;
	auto sampleRange = laser.currentReadingPolar(-20, 20, &sampleAngle);
	auto degreeStr = laser.getDegreesChoicesString();

	std::cout << "auto sampleRange = laser.currentReadingPolar(-20, 20, &sampleAngle);" << sampleRange << std::endl;
	std::cout << "auto degreeStr = laser.getDegreesChoicesString(); : " << degreeStr << std::endl;


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

	// add a set of actions that combine together to effect the wander behavior
	ArActionStallRecover recover;
	ArActionBumpers bumpers;
	ArActionAvoidFront avoidFrontNear("Avoid Front Near", 100, 0);
	ArActionAvoidFront avoidFrontFar;
	ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
	
	robot.addAction(&recover, 100);
	robot.addAction(&bumpers, 75);
	robot.addAction(&avoidFrontNear, 50);
	robot.addAction(&avoidFrontFar, 49);
	robot.addAction(&constantVelocity, 25);

	stream << "IMPORTANT !! == robot.getRobotRadius() : " << robot.getRobotRadius << std::endl;
	std::string timestamp;
	while (1)
	{
		//

		//
		time(&timer);
		strftime(buffer, 80, " - timestamp : %I:%M:%S", localtime(&timer));
		timestamp = buffer;
		dist = robot.checkRangeDevicesCurrentPolar(-70, 70, &sampleAngle) - robot.getRobotRadius();
		stream << count << timestamp << "checkRangeDevicesCurrentPolar(-70, 70, &angle)  :  " << dist << std::endl;
		Sleep(500);
		count++;
	}

	// wwill add processor here

	// wait for robot task loop to end before exiting the program
	robot.waitForRunExit();

	Aria::exit(0);
	return 0;
}

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

  // Connector for laser rangefinders
  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);

  // Connector for compasses
  ArCompassConnector compassConnector(&parser);

  // Parse the command line options. Fail and print the help message if the parsing fails
  // or if the help was requested with the -help option
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {    
    Aria::logOptions();
    exit(1);
  }

  // Used to access and process sonar range data
  ArSonarDevice sonarDev;
  
  // Used to perform actions when keyboard keys are pressed
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);

  // ArRobot contains an exit action for the Escape key. It also 
  // stores a pointer to the keyhandler so that other parts of the program can
  // use the same keyhandler.
  robot.attachKeyHandler(&keyHandler);
  printf("You may press escape to exit\n");

  // Attach sonarDev to the robot so it gets data from it.
  robot.addRangeDevice(&sonarDev);

  
  // Start the robot task loop running in a new background thread. The 'true' argument means if it loses
  // connection the task loop stops and the thread exits.
  robot.runAsync(true);

  // Connect to the laser(s) if lasers were configured in this robot's parameter
  // file or on the command line, and run laser processing thread if applicable
  // for that laser class.  (For the purposes of this demo, add all
  // possible lasers to ArRobot's list rather than just the ones that were
  // specified with the connectLaser option (so when you enter laser mode, you
  // can then interactively choose which laser to use from the list which will
  // show both connected and unconnected lasers.)
  if (!laserConnector.connectLasers(false, false, true))
  {
    printf("Could not connect to lasers... exiting\n");
    Aria::exit(2);
  }

  // Create and connect to the compass if the robot has one.
  ArTCM2 *compass = compassConnector.create(&robot);
  if(compass && !compass->blockingConnect()) {
    compass = NULL;
  }
  
  // Sleep for a second so some messages from the initial responses
  // from robots and cameras and such can catch up
  ArUtil::sleep(1000);

  // We need to lock the robot since we'll be setting up these modes
  // while the robot task loop thread is already running, and they 
  // need to access some shared data in ArRobot.
  robot.lock();

  // now add all the modes for this demo
  // these classes are defined in ArModes.cpp in ARIA's source code.
  ArModeLaser laser(&robot, "laser", 'l', 'L');
  ArModeTeleop teleop(&robot, "teleop", 't', 'T');
  ArModeUnguardedTeleop unguardedTeleop(&robot, "unguarded teleop", 'u', 'U');
  ArModeWander wander(&robot, "wander", 'w', 'W');
  ArModeGripper gripper(&robot, "gripper", 'g', 'G');
  ArModeCamera camera(&robot, "camera", 'c', 'C');
  ArModeSonar sonar(&robot, "sonar", 's', 'S');
  ArModeBumps bumps(&robot, "bumps", 'b', 'B');
  ArModePosition position(&robot, "position", 'p', 'P', &gyro);
  ArModeIO io(&robot, "io", 'i', 'I');
  ArModeActs actsMode(&robot, "acts", 'a', 'A');
  ArModeCommand command(&robot, "command", 'd', 'D');
  ArModeTCM2 tcm2(&robot, "tcm2", 'm', 'M', compass);


  // activate the default mode
  teleop.activate();

  // turn on the motors
  robot.comInt(ArCommands::ENABLE, 1);

  robot.unlock();
  
  // Block execution of the main thread here and wait for the robot's task loop
  // thread to exit (e.g. by robot disconnecting, escape key pressed, or OS
  // signal)
  robot.waitForRunExit();

  Aria::exit(0);


}

//.........这里部分代码省略.........
    robot.addRangeDevice(&sonarDev);

    ArIRs irs;
    robot.addRangeDevice(&irs);

    ArBumpers bumpers;
    robot.addRangeDevice(&bumpers);

    // a laser in case one is used
    ArSick sick(361, 180);
    // add the laser to the robot
    robot.addRangeDevice(&sick);




    // attach stuff to the server
    ArServerInfoRobot serverInfoRobot(&server, &robot);
    ArServerInfoSensor serverInfoSensor(&server, &robot);
    ArServerInfoDrawings drawings(&server);
    drawings.addRobotsRangeDevices(&robot);

    // ways of moving the robot
    ArServerModeStop modeStop(&server, &robot);
    ArServerModeDrive modeDrive(&server, &robot);
    ArServerModeRatioDrive modeRatioDrive(&server, &robot);
    ArServerModeWander modeWander(&server, &robot);
    modeStop.addAsDefaultMode();
    modeStop.activate();

    // set up the simple commands
    ArServerHandlerCommands commands(&server);
    // add the commands for the microcontroller
    ArServerSimpleComUC uCCommands(&commands, &robot);
    // add the commands for logging
    ArServerSimpleComMovementLogging loggingCommands(&commands, &robot);
    // add the commands for the gyro
    ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro);

    // add the commands to enable and disable safe driving to the simple commands
    modeDrive.addControlCommands(&commands);

    // Forward any video if we have some to forward.. this will forward
    // from SAV or ACTS, you can find both on our website
    // http://robots.activmedia.com, ACTS is for color tracking and is
    // charged for but SAV just does software A/V transmitting and is
    // free to all our customers... just run ACTS or SAV before you
    // start this program and this class here will forward video from it
    // to MobileEyes
    ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070);

    // make a camera to use in case we have video
    ArPTZ *camera = NULL;
    ArServerHandlerCamera *handlerCamera = NULL;
    // if we have video then set up a camera
    if (videoForwarder.isForwardingVideo())
    {
        bool invertedCamera = false;
        camera = new ArVCC4(&robot,	invertedCamera,
                            ArVCC4::COMM_UNKNOWN, true, true);
        camera->init();
        handlerCamera = new ArServerHandlerCamera(&server, &robot, camera);
    }

    server.logCommandGroups();
    server.logCommandGroupsToFile("userServerTest.commandGroups");

    // now let it spin off in its own thread
    server.runAsync();

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

    // set up the laser before handing it to the laser mode
    simpleConnector.setupLaser(&sick);

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

    sick.runAsync();

    // connect the laser if it was requested
    if (!simpleConnector.connectLaser(&sick))
    {
        printf("Could not connect to laser... exiting\n");
        Aria::shutdown();
        return 1;
    }

    robot.waitForRunExit();
    // now exit
    Aria::shutdown();
    exit(0);
}

int main(int argc, char **argv)
{
  Aria::init();

  // parse our args and make sure they were all accounted for
  ArSimpleConnector connector(&argc, argv);

  ArRobot robot;

  // the laser. ArActionTriangleDriveTo will use this laser object since it is
  // named "laser" when added to the ArRobot.
  ArSick sick;

  if (!connector.parseArgs() || argc > 1)
  {
    connector.logOptions();
    exit(1);
  }
  
  // a key handler so we can do our key handling
  ArKeyHandler keyHandler;
  // let the global aria stuff know about it
  Aria::setKeyHandler(&keyHandler);
  // toss it on the robot
  robot.attachKeyHandler(&keyHandler);

  // add the laser to the robot
  robot.addRangeDevice(&sick);

  ArSonarDevice sonar;
  robot.addRangeDevice(&sonar);
  
  ArActionTriangleDriveTo triangleDriveTo;
  ArFunctorC<ArActionTriangleDriveTo> lineGoCB(&triangleDriveTo, 
				      &ArActionTriangleDriveTo::activate);
  keyHandler.addKeyHandler('g', &lineGoCB);
  keyHandler.addKeyHandler('G', &lineGoCB);
  ArFunctorC<ArActionTriangleDriveTo> lineStopCB(&triangleDriveTo, 
					&ArActionTriangleDriveTo::deactivate);
  keyHandler.addKeyHandler('s', &lineStopCB);
  keyHandler.addKeyHandler('S', &lineStopCB);

  ArActionLimiterForwards limiter("limiter", 150, 0, 0, 1.3);
  robot.addAction(&limiter, 70);
  ArActionLimiterBackwards limiterBackwards;
  robot.addAction(&limiterBackwards, 69);

  robot.addAction(&triangleDriveTo, 60);

  ArActionKeydrive keydrive;
  robot.addAction(&keydrive, 55);


  ArActionStop stopAction;
  robot.addAction(&stopAction, 50);
  
  // 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, 1);
  robot.comInt(ArCommands::ENABLE, 1);
  
  // start the robot running, true so that if we lose connection the run stops
  robot.runAsync(true);

  // now set up the laser
  connector.setupLaser(&sick);

  sick.runAsync();

  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    Aria::shutdown();
    return 1;
  }

  printf("If you press the 'g' key it'll go find a triangle, if you press 's' it'll stop.\n");

  robot.waitForRunExit();
  return 0;
}

int main(int argc, char **argv)
{
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArRobot robot;
  ArRobotConnector robotConnector(&parser, &robot);
  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);

  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "lineFinderExample: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
        // -help not given
        Aria::logOptions();
        Aria::exit(1);
    }
  }

  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::exit(1);
  }

  ArLog::log(ArLog::Normal, "lineFinderExample: Connected to robot.");

  robot.runAsync(true);

  // Connect to laser(s) as defined in parameter files.
  // (Some flags are available as arguments to connectLasers() to control error behavior and to control which lasers are put in the list of lasers stored by ArRobot. See docs for details.)
  if(!laserConnector.connectLasers())
  {
    ArLog::log(ArLog::Terse, "Could not connect to configured lasers. Exiting.");
    Aria::exit(3);
    return 3;
  }

  ArLog::log(ArLog::Normal, "lineFinderExample: Connected to laser");

  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);


  // Create the ArLineFinder object. Set it to log lots of information about its
  // processing.

  ArLaser *laser = robot.findLaser(1);
  if(!laser)
  {
    ArLog::log(ArLog::Terse, "lineFinderExample: No laser device connected, exiting.");
    Aria::exit(4);
    return 4;
  }

  ArLineFinder lineFinder(laser);
  lineFinder.setVerbose(true);

  // Add key callbacks that simply call the ArLineFinder::getLinesAndSaveThem()
  // function, which searches for lines in the current set of laser sensor
  // readings, and saves them in files with the names 'points' and 'lines'.
  ArFunctorC<ArLineFinder> findLineCB(&lineFinder, 
				  &ArLineFinder::getLinesAndSaveThem);
  keyHandler.addKeyHandler('f', &findLineCB);
  keyHandler.addKeyHandler('F', &findLineCB);

  
  printf("If you press the 'f' key the points and lines found will be saved\n");
  printf("Into the 'points' and 'lines' file in the current working directory\n");

  robot.waitForRunExit();
  Aria::exit(0);
  return 0;
}

int main(int argc, char **argv)
{
  Aria::init();

  ArSimpleConnector connector(&argc, argv);
  ArRobot robot;
  ArSick sick;

  if (!Aria::parseArgs() || argc > 1)
  {
    Aria::logOptions();
    Aria::exit(1); // exit program with error code 1
    return 1;
  }
  
  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);


  robot.addRangeDevice(&sick);

  // Create the ArLineFinder object. Set it to log lots of information about its
  // processing.
  ArLineFinder lineFinder(&sick);
  lineFinder.setVerbose(true);

  // Add key callbacks that simply call the ArLineFinder::getLinesAndSaveThem()
  // function, which searches for lines in the current set of laser sensor
  // readings, and saves them in files with the names 'points' and 'lines'.
  ArFunctorC<ArLineFinder> findLineCB(&lineFinder, 
				  &ArLineFinder::getLinesAndSaveThem);
  keyHandler.addKeyHandler('f', &findLineCB);
  keyHandler.addKeyHandler('F', &findLineCB);

  
  ArLog::log(ArLog::Normal, "lineFinderExample: connecting to robot...");
  if (!connector.connectRobot(&robot))
  {
    printf("Could not connect to robot... exiting\n");
    Aria::exit(1);  // exit program with error code 1
    return 1;
  }
  robot.runAsync(true);

  // now set up the laser
  ArLog::log(ArLog::Normal, "lineFinderExample: connecting to SICK laser...");
  connector.setupLaser(&sick);
  sick.runAsync();
  if (!sick.blockingConnect())
  {
    printf("Could not connect to SICK laser... exiting\n");
    Aria::exit(1);
    return 1;
  }

  printf("If you press the 'f' key the points and lines found will be saved\n");
  printf("Into the 'points' and 'lines' file in the current working directory\n");

  robot.waitForRunExit();
  Aria::exit(0);
  return 0;
}

int main(int argc, char **argv)
{
  Aria::init();
  ArArgumentParser argParser(&argc, argv);
  argParser.loadDefaultArguments();
  ArRobot robot;
  ArRobotConnector robotConnector(&argParser, &robot);
  ArLaserConnector laserConnector(&argParser, &robot, &robotConnector);

  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "Could not connect to the robot.");
    if(argParser.checkHelpAndWarnUnparsed())
    {
        // -help not given, just exit.
        Aria::logOptions();
        Aria::exit(1);
        return 1;
    }
  }


  // Trigger argument parsing
  if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    Aria::exit(1);
    return 1;
  }

  ArKeyHandler keyHandler;
  Aria::setKeyHandler(&keyHandler);
  robot.attachKeyHandler(&keyHandler);

  puts("This program will make the robot wander around. It uses some avoidance\n"
  "actions if obstacles are detected, otherwise it just has a\n"
  "constant forward velocity.\n\nPress CTRL-C or Escape to exit.");
  
  ArSonarDevice sonar;
  robot.addRangeDevice(&sonar);

  robot.runAsync(true);

  
  // try to connect to laser. if fail, warn but continue, using sonar only
  if(!laserConnector.connectLasers())
  {
    ArLog::log(ArLog::Normal, "Warning: unable to connect to requested lasers, will wander using robot sonar only.");
  }


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

  // add a set of actions that combine together to effect the wander behavior //działa
	//general structure: robot.addAction (new Name_of_action (parameters), priority);
	//to find out more about Actions go to ~/catkin_ws/src/rosaria/local/Aria/docs/index.html

// if we're stalled we want to back up and recover
  robot.addAction(new ArActionStallRecover("stall recover", 100,50,100,30), 100); //basic values 225, 50, 150, 45
								//when_stop, cycles_to_move, speed, turn'
//slow down when near obstacle
  robot.addAction(new ArActionLimiterForwards("speed limiter near", 
						200, 200, 200), 95); // basic values 300,600,250
								//stop_distance, slow_distance, slow_speed
  // react to bumpers
  robot.addAction(new ArActionLimiterBackwards, 75);

// turn avoid very close
  robot.addAction(new ArActionAvoidFront("speed limiter far", 150,450,20), 47);	//basic values 450, 200,15
						//when_turn, speed, turn'

// turn avoid things near
  robot.addAction(new ArActionAvoidFront("speed limiter far", 300,450,10), 46);	//basic values 450, 200,15
						//when_turn, speed, turn'

// turn avoid things further away
  robot.addAction(new ArActionAvoidFront("speed limiter far", 800,450,5), 45);	//basic values 450, 200,15
						//when_turn, speed, turn'
  
//move forward
  robot.addAction(new ArActionConstantVelocity("Constant Velocity", 500), 25);



  
  // wait for robot task loop to end before exiting the program
  robot.waitForRunExit();
  
  Aria::exit(0);
  return 0;
}

//.........这里部分代码省略.........
  //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();
  robot.comInt(ArCommands::IOREQUEST, 1);
  robot.comInt(ArCommands::ENABLE, 1);
  robot.unlock();

  robot.waitForRunExit();
  // now exit
  Aria::shutdown();
  return 0;
}

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


    /* Load configuration values, map, and begin! */

  
  // When parsing the configuration file, also look at the program's command line options 
  // from the command-line argument parser as well as the configuration file.
  // (So you can use any argument on the command line, namely -map.) 
  Aria::getConfig()->useArgumentParser(&parser);

  // Read in parameter files.
  ArLog::log(ArLog::Normal, "Loading config file %s%s into ArConfig...", Aria::getDirectory(), Arnl::getTypicalParamFileName());
  if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
  {
    ArLog::log(ArLog::Normal, "Could not load ARNL configuration file. Set ARNL environment variable to use non-default installation director.y");
    Aria::exit(5);
  }

  // Warn about unknown params.
  if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
  {
    logOptions(argv[0]);
    Aria::exit(6);
  }

  // Warn if there is no map
  if (map.getFileName() == NULL || strlen(map.getFileName()) <= 0)
  {
    ArLog::log(ArLog::Normal, "");
    ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure");
#ifdef ARNL
    ArLog::log(ArLog::Normal, "   0) You can find this information in README.txt or docs/Mapping.txt");
    ArLog::log(ArLog::Normal, "   1) Connect to this server with MobileEyes");
    ArLog::log(ArLog::Normal, "   2) Go to Tools->Map Creation->Start Scan");
    ArLog::log(ArLog::Normal, "   3) Give the map a name and hit okay");
    ArLog::log(ArLog::Normal, "   4) Drive the robot around your space (see docs/Mapping.txt");
    ArLog::log(ArLog::Normal, "   5) Go to Tools->Map Creation->Stop Scan");
    ArLog::log(ArLog::Normal, "   6) Start up Mapper3");
    ArLog::log(ArLog::Normal, "   7) Go to File->Open on Robot");
    ArLog::log(ArLog::Normal, "   8) Select the .2d you created");
    ArLog::log(ArLog::Normal, "   9) Create a .map");
    ArLog::log(ArLog::Normal, "  10) Go to File->Save on Robot");
    ArLog::log(ArLog::Normal, "  11) In MobileEyes, go to Tools->Robot Config");
    ArLog::log(ArLog::Normal, "  12) Choose the Files section");
    ArLog::log(ArLog::Normal, "  13) Enter the path and name of your new .map file for the value of the Map parameter.");
    ArLog::log(ArLog::Normal, "  14) Press OK and your new map should become the map used");
    ArLog::log(ArLog::Normal, "");    
#elif defined(SONARNL)
    ArLog::log(ArLog::Normal, "   0) You can find this information in README.txt or docs/SonarMapping.txt");
    ArLog::log(ArLog::Normal, "   1) Start up Mapper3Basic");
    ArLog::log(ArLog::Normal, "   2) Go to File->New");
    ArLog::log(ArLog::Normal, "   3) Draw a line map of your area (make sure it is to scale)");
    ArLog::log(ArLog::Normal, "   4) Go to File->Save on Robot");
    ArLog::log(ArLog::Normal, "   5) In MobileEyes, go to Tools->Robot Config");
    ArLog::log(ArLog::Normal, "   6) Choose the Files section");
    ArLog::log(ArLog::Normal, "   7) Enter the path and name of your new .map file for the value of the Map parameter.");
    ArLog::log(ArLog::Normal, "   8) Press OK and your new map should become the map used");
    ArLog::log(ArLog::Normal, "");    
#endif
#ifdef ARNL_GPSLOC
    ArLog::log(ArLog::Normal, "\n   See docs/GPSMapping.txt for instructions on creating a map for GPS localization");
#endif
  }

  // Print a log message notifying user of the directory for map files
  ArLog::log(ArLog::Normal, "");
  ArLog::log(ArLog::Normal, 
	     "Directory for maps and file serving: %s", fileDir);
  
  ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
  ArLog::log(ArLog::Normal, "");

  // Do an initial localization of the robot. ARNL and SONARNL try all the home points
  // in the map, as well as the robot's current odometric position, as possible
  // places the robot is likely to be at startup.   If successful, it will
  // also save the position it found to be the best localized position as the
  // "Home" position, which can be obtained from the localization task (and is
  // used by the "Go to home" network request).
  // MOGS instead just initializes at the current GPS position.
  // (You will stil have to drive the robot so it can determine the robot's
  // heading, however. See GPS Mapping instructions.)
  LOCTASK.localizeRobotAtHomeBlocking();
  
#ifdef ARNL_MULTIROBOT
  // Let the client switch manager (for multirobot) spin off into its own thread
  // TODO move to multirobot example?
  clientSwitch.runAsync();
#endif

  // Start the networking server's thread
  server.runAsync();

  ArLog::log(ArLog::Normal, "Server running. To exit, press CTRL-C.");

  // Enable the motors and wait until the robot exits (disconnection, etc.) or this program is
  // canceled.
  robot.enableMotors();
  robot.waitForRunExit();
  Aria::exit(0);
}

int main(int argc, char** argv)
{
  Aria::init();
  ArLog::init(ArLog::StdErr, ArLog::Normal);

  ArArgumentParser argParser(&argc, argv);
  ArSimpleConnector connector(&argParser);
  ArGPSConnector gpsConnector(&argParser);
  ArRobot robot;

  ArActionLimiterForwards nearLimitAction("limit near", 300, 600, 250);
  ArActionLimiterForwards farLimitAction("limit far", 300, 1100, 400);
  ArActionLimiterBackwards limitBackwardsAction;
  ArActionJoydrive joydriveAction;
  ArActionKeydrive keydriveAction;

  ArSonarDevice sonar;
  ArSick laser;

  argParser.loadDefaultArguments();
  if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
  {
    Aria::logOptions();
    return -1;
  }

  robot.addRangeDevice(&sonar);
  robot.addRangeDevice(&laser);

  ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connecting to robot...");
  if(!connector.connectRobot(&robot))
  {
    ArLog::log(ArLog::Terse, "gpsRobotTaskExample: Could not connect to the robot. Exiting.");
    return -2;
  }
  ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connected to the robot.");


  // Connect to GPS
  ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connecting to GPS, it may take a few seconds...");
  ArGPS *gps = gpsConnector.createGPS();
  if(!gps || !gps->connect());
  {
    ArLog::log(ArLog::Terse, "gpsRobotTaskExample: Error connecting to GPS device.  Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments. Use -help for help. Exiting.");
    return -3;
  }


  // Create an GPSLogTask which will register a task with the robot.
  GPSLogTask gpsTask(&robot, gps, joydriveAction.getJoyHandler()->haveJoystick() ? joydriveAction.getJoyHandler() : NULL);


  // Add actions
  robot.addAction(&nearLimitAction, 100);
  robot.addAction(&farLimitAction, 90);
  robot.addAction(&limitBackwardsAction, 80);
  robot.addAction(&joydriveAction, 50);
  robot.addAction(&keydriveAction, 40);

  // allow keydrive action to drive robot even if joystick button isn't pressed
  joydriveAction.setStopIfNoButtonPressed(false);

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

  // Connect to the laser
  connector.setupLaser(&laser);
  laser.runAsync();
  if(!laser.blockingConnect())
    ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Warning, could not connect to SICK laser, will not use it.");

  robot.lock();

  robot.enableMotors();
  robot.comInt(47, 1);  // enable joystick driving on some robots

  // Add exit callback to reset/unwrap steering wheels on seekur (critical if the robot doesn't have sliprings); does nothing for other robots 
  Aria::addExitCallback(new ArRetFunctor1C<bool, ArRobot, unsigned char>(&robot, &ArRobot::com, (unsigned char)120));
  Aria::addExitCallback(new ArRetFunctor1C<bool, ArRobot, unsigned char>(&robot, &ArRobot::com, (unsigned char)120));

  robot.unlock();

  ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Running... (drive robot with joystick or arrow keys)");
  robot.waitForRunExit();


  return 0;
}

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

  ArServerInfoRobot serverInfoRobot(&server, &robot);
  ArServerInfoSensor serverInfoSensor(&server, &robot);

  // This is the service that provides drawing data to the client.
  ArServerInfoDrawings drawings(&server);

  // Convenience function that sets up drawings for all the robot's current
  // range devices (using default shape and color info)
  drawings.addRobotsRangeDevices(&robot);

  // Add our custom drawings
  drawings.addDrawing(
      //                shape:      color:               size:   layer:
      new ArDrawingData("polyLine", ArColor(255, 0, 0),  2,      49),
      "exampleDrawing_Home", 
      new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleHomeDrawingNetCallback)
  );
  drawings.addDrawing(                                    
      new ArDrawingData("polyDots", ArColor(0, 255, 0), 250, 48),
      "exampleDrawing_Dots", 
      new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleDotsDrawingNetCallback)
  );
  drawings.addDrawing(
      new ArDrawingData("polySegments", ArColor(0, 0, 0), 4, 52),
      "exampleDrawing_XMarksTheSpot", 
      new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleXDrawingNetCallback)
  );
  drawings.addDrawing(
      new ArDrawingData("polyArrows", ArColor(255, 0, 255), 500, 100),
      "exampleDrawing_Arrows", 
      new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleArrowsDrawingNetCallback)
  );

  // modes for moving the robot
  ArServerModeStop modeStop(&server, &robot);
  ArServerModeDrive modeDrive(&server, &robot);
  ArServerModeRatioDrive modeRatioDrive(&server, &robot);
  ArServerModeWander modeWander(&server, &robot);
  modeStop.addAsDefaultMode();
  modeStop.activate();

  

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

  // set up the laser before handing it to the laser mode
  simpleConnector.setupLaser(&sick);

  robot.enableMotors();

  // start the robot cycle running in a background thread
  robot.runAsync(true);

  // start the laser processing cycle in a background thread
  sick.runAsync();

  // connect the laser if it was requested
  if (!simpleConnector.connectLaser(&sick))
  {
    printf("Error: Could not connect to laser... exiting\n");
    Aria::shutdown();
    return 1;
  }

  // log whatever we wanted to before the runAsync
  simpleOpener.checkAndLog();

  // run the server thread in the background
  server.runAsync();

  printf("Server is now running...\n");


  // Add a key handler mostly that windows can exit by pressing
  // escape, note that the key handler prevents you from running this program
  // in the background on Linux.
  ArKeyHandler *keyHandler;
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    keyHandler = new ArKeyHandler;
    Aria::setKeyHandler(keyHandler);
    robot.lock();
    robot.attachKeyHandler(keyHandler);
    robot.unlock();
    printf("To exit, press escape.\n");
  }

  robot.waitForRunExit();
 

  Aria::shutdown();
  exit(0);  
}