C++ ArSick类代码示例

// Collects a laser reading from the robot, and corrects the XY-rotation
vector<PointXY> CollectLaser(ArSick &sick, double maxRange)
{
	vector<PointXY> laserPoints;

	sick.lockDevice();

	vector<ArSensorReading> * readings = sick.getRawReadingsAsVector();
	for(vector<ArSensorReading>::const_iterator it = readings->begin(); it != readings->end(); it++)
	{
		if(it->getRange() < maxRange)
		{
			// Correct coordinates right here so we don't have to do it later!
			double x = -it->getLocalY();
			double y =  it->getLocalX();
			laserPoints.push_back(PointXY(x, y));
		}
	}

	sick.unlockDevice();
	//ArUtil::sleep(100);

	return laserPoints;
}

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)
{
  int ret;
  std::string str;
  // the serial connection (robot)
  ArSerialConnection serConn;
  // tcp connection (sim)
  ArTcpConnection tcpConn;
  // the robot
  ArRobot robot;
  // the laser
  ArSick sick;
  // the laser connection
  ArSerialConnection laserCon;

  bool useSimForLaser = false;


  std::string hostname = "prod.local.net";

  // timeouts in minutes
  int wanderTime = 0;
  int restTime = 0;


  // check arguments
  if (argc == 3 || argc == 4)
  {
    wanderTime = atoi(argv[1]);
    restTime = atoi(argv[2]);
    if (argc == 4)
      hostname = argv[3];
  }
  else
  {
    printf("\nUsage:\n\tpeoplebotTest <wanderTime> <restTime> <hostname>\n\n");
    printf("Times are in minutes.  Hostname is the machine to pipe the ACTS display to\n\n");
    wanderTime = 15;
    restTime = 45;
  }

  printf("Wander time - %d minutes\nRest time - %d minutes\n", wanderTime, restTime);
  printf("Sending display to %s.\n\n", hostname.c_str());

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

  // the actions we'll use to wander
  ArActionStallRecover recover;
  ArActionBumpers bumpers;
  ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 0);
  ArActionAvoidFront avoidFrontFar;

  // Make a key handler, so that escape will shut down the program
  // cleanly
  ArKeyHandler keyHandler;

  // mandatory init
  Aria::init();

  // Add the key handler to Aria so other things can find it
  Aria::setKeyHandler(&keyHandler);

  // Attach the key handler to a robot now, so that it actually gets
  // some processing time so it can work, this will also make escape
  // exit
  robot.attachKeyHandler(&keyHandler);


  // First we see if we can open the tcp connection, if we can we'll
  // assume we're connecting to the sim, and just go on...  if we
  // can't open the tcp it means the sim isn't there, so just try the
  // robot

  // modify this next line if you're not using default tcp connection
  tcpConn.setPort();

  // see if we can get to the simulator  (true is success)
  if (tcpConn.openSimple())
  {
    // we could get to the sim, so set the robots device connection to the sim
    printf("Connecting to simulator through tcp.\n");
    robot.setDeviceConnection(&tcpConn);
  }
  else
  {
    // we couldn't get to the sim, so set the port on the serial
    // connection and then set the serial connection as the robots
    // device

    // modify the next line if you're not using the first serial port
    // to talk to your robot
    serConn.setPort();
    printf(
      "Could not connect to simulator, connecting to robot through serial.\n");
    robot.setDeviceConnection(&serConn);
  }
  
  
  // add the sonar to the robot
//.........这里部分代码省略.........

int main(int argc, char **argv) 
{
	struct settings robot_settings;

	robot_settings.min_distance = 500;
	robot_settings.max_velocity = 500;
	robot_settings.tracking_factor = 1.0;

	ArRobot robot;
	Aria::init();
	//laser
	int ret; //Don't know what this variable is for
	ArSick sick; // Laser scanner
	ArSerialConnection laserCon; // Scanner connection
	std::string str; // Standard output
	// sonar, must be added to the robot
	ArSonarDevice sonar;
	// add the sonar to the robot
	robot.addRangeDevice(&sonar);
	// add the laser to the robot
	robot.addRangeDevice(&sick);

	ArArgumentParser argParser(&argc, argv);
	ArSimpleConnector con(&argParser);

	// the connection handler from above
	ConnHandler ch(&robot);

	if(!Aria::parseArgs())
	{
		Aria::logOptions();
		Aria::shutdown();
		return 1;
	}
	if(!con.connectRobot(&robot))
	{
		ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting.");
		return 1;
	}
	ArLog::log(ArLog::Normal, "directMotionExample: Connected.");
	robot.runAsync(false);

	///////////////////////////////
	// Attempt to connect to SICK using another hard-coded USB connection
	sick.setDeviceConnection(&laserCon);
	if((ret=laserCon.open("/dev/ttyUSB1")) !=0){
		//If connection fails, shutdown
		Aria::shutdown();
		return 1;
	}
	//Configure the SICK
	sick.configureShort(false,/*not using sim*/ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_HALF);
	//Run the sick
	sick.runAsync();
	// Presumably test to make sure that the connection is good
	if(!sick.blockingConnect()){
		printf("Could not get sick...exiting\n");
		Aria::shutdown();
		return 1; 
	}
	printf("We are connected to the laser!");

	printf("\r\nRobot Entering default resting state.\r\nUse the following commands to run the robot.\r\n");
	printf("r  Run the robot\r\n");
	printf("s  Stop the robot\r\n");
	printf("t  Enter test mode (the robot will do everything except actually move.\r\n");
	printf("w  Save current data to files, and show a plot of what the robot sees.\r\n");
	printf("e  Edit robot control parameters\r\n");
	printf("q  Quit the program\r\n");

	printf("\r\n");
	printf("\r\n");
	printf("NOTE: You must have GNUPLOT installed on your computer, and create a directory entitled 'scan_data' under your current directory in order to use this script. Failure to do so might make the computer crash, which would cause the robot to go on a mad killing spree! Not really, but seriously, go ahead and get GNUPLOT and create that subdirectory before using the 'w' option.\r\n\r\n"); 

	/////////////////////////////////////
	char user_command = 0;
	char plot_option = 0;

	int robot_state = REST;

	tracking_object target;
	tracking_object l_target;

	ofstream fobjects;
	ofstream ftarget;
	ofstream flog;
	ofstream fltarget;


	fobjects.open("./scan_data/objects_new.txt");
	ftarget.open("./scan_data/target_new.txt");
	fltarget.open("./scan_data/ltarget_new.txt");
	fobjects << "\r\n";
	ftarget << "\r\n";
	fltarget << "\r\n";
	fobjects.close();
	ftarget.close();
	fltarget.close();

	flog.open("robot_log.txt");
//.........这里部分代码省略.........

int main(int argc, char **argv)
{
  bool done;
  double distToTravel = 2300;

  // whether to use the sim for the laser or not, if you use the sim
  // for hte laser, you have to use the sim for the robot too
  bool useSim = false;
  // the laser
  ArSick sick;
  // connection
  ArDeviceConnection *con;
  // Laser connection
  ArSerialConnection laserCon;
  // robot
  ArRobot robot;

  // set a default filename
  //std::string filename = "c:\\log\\1scans.2d";
  std::string filename = "1scans.2d";
  // see if we want to use a different filename
  //if (argc > 1)
  //Lfilename = argv[1];
  printf("Logging to file %s\n", filename.c_str());
  // start the logger with good values
  sick.configureShort(useSim, ArSick::BAUD38400,
		 ArSick::DEGREES180, ArSick::INCREMENT_HALF);
  ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str());
  
  // mandatory init
  Aria::init();

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

  //ArAnalogGyro gyro(&robot);


  // if we're not using the sim, make a serial connection and set it up
  if (!useSim)
  {
    ArSerialConnection *serCon;
    serCon = new ArSerialConnection;
    serCon->setPort();
    //serCon->setBaud(38400);
    con = serCon;
  }
  // if we are using the sim, set up a tcp connection
  else
  {
    ArTcpConnection *tcpCon;
    tcpCon = new ArTcpConnection;
    tcpCon->setPort();
    con = tcpCon;
  }

  // set the connection on the robot
  robot.setDeviceConnection(con);
  // try to connect, if we fail exit
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }


  // set up a key handler so escape exits and attach to the robot
  ArKeyHandler keyHandler;
  robot.attachKeyHandler(&keyHandler);

  // run the robot, true here so that the run will exit if connection lost
  robot.runAsync(true);



  // if we're not using the sim, set up the port for the laser
  if (!useSim)
  {
    laserCon.setPort(ArUtil::COM3);
    sick.setDeviceConnection(&laserCon);
  }


  // 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");
    robot.disconnect();
    Aria::shutdown();
    return 1;
  }

#ifdef WIN32
  // wait until someone pushes the motor button to go
  while (1)
  {
//.........这里部分代码省略.........

int main(int argc, char **argv)
{

	int t, cnt;
	double laser_dist[900];
	double laser_angle[900];
	std::list<ArSensorReading *> *readings;
	std::list<ArSensorReading *>::iterator it;


	ArKeyHandler keyHandler;

	Aria::init();

	// Add the key handler to Aria so other things can find it
	Aria::setKeyHandler(&keyHandler);
	robot.attachKeyHandler(&keyHandler);

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

	// Parse all our args
	ArSimpleConnector connector(&argc, argv);

	if (!connector.parseArgs() || argc > 1)
	{
		connector.logOptions();
		exit(1);
	}

	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;
	}


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

	// now set up the laser
	sick.configureShort(true,ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_ONE);

	connector.setupLaser(&sick);

	sick.runAsync();

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


    cnt = 1;
	while(cnt<10000){
		readings=(list<ArSensorReading *,allocator<ArSensorReading *> > *)sick.getRawReadings();//CurrentBuffer..
		while (readings == NULL){
			readings = (list<ArSensorReading *, allocator<ArSensorReading *> > *)sick.getRawReadings();
		}
			t=0;
			for(it=readings->begin(); it!=readings->end(); it++){
				//cout << "t: " << t << endl;
				laser_dist[t]=(*it)->getRange();
				laser_angle[t]=-90+t;
				//cout << "laser angle: " << laser_angle[t] << " laser dist.: " << laser_dist[t] <<" "<<"\n";
				t++;
			}
			cout << "count: " << cnt << endl; //for some reason this line needs to be here
			cnt++;
						
	}
	
    for (t=0; t<181; t++){
		cout << "laser angle: " << laser_angle[t] << " laser dist.: " << laser_dist[t] <<" "<<"\n";
    }
	
	robot.waitForRunExit();
	Aria::shutdown();
	return 0;
}

int main(int argc, char **argv)
{
    int ret; //Don't know what this variable is for
    
    ArRobot robot;// Robot object
    
    ArSick sick; // Laser scanner
    ArSerialConnection laserCon; // Scanner connection
    
    ArSerialConnection con; // Robot connection
    
    std::string str; // Standard output
    
    
    // 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", 300, 600, 250, 1.1);
    // limiter for far away obstacles
    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", 400);
    // turn the orbot if its slowed down
    ArActionTurn turn;
    
    
    
    
    // mandatory init
    Aria::init();
    
    // Parse all our args
    ArSimpleConnector connector(&argc, argv);
    connector.parseArgs();
    
    if (argc > 1)
    {
        connector.logOptions();
        exit(1);
    }
    
    // add the sonar to the robot
    robot.addRangeDevice(&sonar);
    // add the laser to the robot
    robot.addRangeDevice(&sick);
    
    // NOTE: HARDCODED USB PORT!
    // Attempt to open hard-coded USB to robot
    if ((ret = con.open("/dev/ttyUSB2")) != 0){
        // If connection fails, exit
        str = con.getOpenMessage(ret);
        printf("Open failed: %s\n", str.c_str());
        Aria::shutdown();
        return 1;
    }
    
    // set the robot to use the given connection
    robot.setDeviceConnection(&con);
    
    // do a blocking connect, if it fails exit
    if (!robot.blockingConnect())
    {
        printf("Could not connect to robot... exiting\n");
        Aria::shutdown();
        return 1;
    }
    
    
    
    
    // turn on the motors, turn off amigobot sounds
    //robot.comInt(ArCommands::SONAR, 0);
    robot.comInt(ArCommands::SOUNDTOG, 0);
    
    
    
    // start the robot running, true so that if we lose connection the run stops
    robot.runAsync(true);
    
    
    // Attempt to connect to SICK using another hard-coded USB connection
    sick.setDeviceConnection(&laserCon);
    if((ret=laserCon.open("/dev/ttyUSB3")) !=0) {
        //If connection fails, shutdown
        Aria::shutdown();
        return 1;
    }
    
    //Configure the SICK
    sick.configureShort(false,/*not using sim*/ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_HALF);
    
    //Run the sick
//.........这里部分代码省略.........

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)
{
  // 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();
//.........这里部分代码省略.........

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;
}

int main(int argc, char **argv)
{
  double speed = 1000;
  double squareSide = 2000;

  // whether to use the sim for the laser or not, if you use the sim
  // for hte laser, you have to use the sim for the robot too
  // robot
  robot = new ArRobot;
  // the laser
  ArSick sick;
  // set up our simpleConnector
  ArSimpleConnector simpleConnector(&argc, argv);

  // set up a key handler so escape exits and attach to the robot
  ArKeyHandler keyHandler;
  robot->attachKeyHandler(&keyHandler);

  // parse its arguments
  if (simpleConnector.parseArgs())
  {
    simpleConnector.logOptions();
    keyHandler.restore();
    exit(1);
  }

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

  // set a default filename
  //std::string filename = "c:\\log\\1scans.2d";
  std::string filename = "1scans.2d";
  // see if we want to use a different filename
  //if (argc > 1)
  //Lfilename = argv[1];
  printf("Logging to file %s\n", filename.c_str());
  // start the logger with good values
  
  //sick.configureShort(useSim, ArSick::BAUD38400, 	 ArSick::DEGREES180, ArSick::INCREMENT_HALF);
  ArSickLogger logger(robot, &sick, 300, 25, filename.c_str());
  
  // mandatory init
  Aria::init();

  // add it to the robot
  robot->addRangeDevice(&sick);

  //ArAnalogGyro gyro(robot);


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

  robot->setRotVelMax(300);
  robot->setRotAccel(300);
  robot->setRotDecel(300);

  robot->setAbsoluteMaxTransVel(2000);
  robot->setTransVelMax(2000);
  robot->setTransAccel(500);
  robot->setTransDecel(500);
  /*
  robot->comInt(82, 30); // rotkp
  robot->comInt(83, 200); // rotkv
  robot->comInt(84, 0); // rotki
  robot->comInt(85, 15); // transkp
  robot->comInt(86, 450); // transkv
  robot->comInt(87, 4); // transki
  */
  robot->comInt(82, 30); // rotkp
  robot->comInt(83, 200); // rotkv
  robot->comInt(84, 0); // rotki
  robot->comInt(85, 30); // transkp
  robot->comInt(86, 450); // transkv
  robot->comInt(87, 4); // transki
  // run the robot, true here so that the run will exit if connection lost
  robot->runAsync(true);



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

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

 if (!sick.blockingConnect())
//.........这里部分代码省略.........

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

   ArRobot robot;
   ArSick sick;
   double dist, angle = 0;

   // Allow for esc to release robot
   ArKeyHandler keyHandler;
   Aria::setKeyHandler(&keyHandler);
   robot.attachKeyHandler(&keyHandler);
   printf("You may press escape to exit\n");

   if( !connector.parseArgs() || argc > 1 ){
      connector.logOptions();
      exit(1);
   }

   // 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;
   }

   // 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;
   }

   robot.comInt(ArCommands::ENABLE, 1);
   ArPose pose(0, -1000, 0);
   robot.moveTo(pose);
   ArPose prev_pose = robot.getPose();
   double total_distance = 0;

   printf("Connected\n");
   ArUtil::sleep(1000);

   PathLog log("../Data/reactive.dat");
   int iterations_wo_movement = 0;
   while( iterations_wo_movement < CONSECUTIVE_NON_MOTIONS ){

      // Get updated measurement
      sick.lockDevice();
      dist = sick.currentReadingPolar(-90, 90, &angle);
      sick.unlockDevice();

      dist = (dist > 30000) ? 0 : dist - IDEAL_DISTANCE;
      trackRobot(&robot, dist, angle);
      ArUtil::sleep(500);
      pose = robot.getPose();
      log.write(pose);
      total_distance += getDistance(prev_pose, pose);
      prev_pose = pose;

      // Determine if the robot is done tracking
      isRobotTracking(&iterations_wo_movement, dist, angle);

   }

   ArUtil::sleep(1000);
   log.close();

   ofstream output;
   output.open("../Data/reactive_dist.dat", ios::out | ios::trunc);
   output << "Reactive 1 " << total_distance << endl;
   output.close();

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

int main(int argc, char **argv)
{
	Aria::init();
	ArArgumentParser argParser(&argc, argv);
	argParser.loadDefaultArguments();
	ArRobot robot;
	ArSick laser;
	std::ofstream stream; // for loggin
	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++;
	}

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

/*!
 * Shuts down the system.
 *
 */
void
Advanced::shutDown(void)
{
  Aria::exit(0);
  //
  // Stop the path planning thread.
  //
  if(myPathPlanningTask){
    myPathPlanningTask->stopRunning();
//    delete myPathPlanningTask;
    printf("Stopped Path Planning Thread\n");
  }
  //
  // Stop the localization thread.
  //
  if(myLocaTask){
    myLocaTask->stopRunning();
    delete myLocaTask;
    printf("Stopped Localization Thread\n");
  }
  //
  // Stop the laser thread.
  //
  if(mySick)
  {
    mySick->lockDevice();
    mySick->disconnect();
    mySick->unlockDevice();
    printf("Stopped Laser Thread\n");
  }
  //
  // Stop the robot thread.
  //
  myRobot->lock();
  myRobot->stopRunning();
  myRobot->unlock();
  printf("Stopped Robot Thread\n");
  //
  // Exit Aria
  //
  Aria::shutdown();
  printf("Aria Shutdown\n");

}

int main(int argc, char **argv)
{

//----------------------initialized robot server------------------------------------------
  Aria::init();
  Arnl::init();
  
  ArServerBase server;
	
//-----------------------------------------------------------------------------------
	VCCHandler ptz(&robot); 		//create keyboard for control vcc50i

	
	G_PTZHandler->reset();
	ArUtil::sleep(300);
	G_PTZHandler->panSlew(30);
//-----------------------------------------------------------------------------------

  argc = 2 ;

  argv[0] = "-map";
  argv[1] = "map20121111.map";
  
  // Parse the command line arguments.
  ArArgumentParser parser(&argc, argv);

  // Set up our simpleConnector
  ArSimpleConnector simpleConnector(&parser);

  // Set up our simpleOpener
  ArServerSimpleOpener simpleOpener(&parser);
  

//*******
  // Set up our client for the central server
//   ArClientSwitchManager clientSwitch(&server, &parser);
//************
  
  // Load default arguments for this computer (from /etc/Aria.args, environment
  // variables, and other places)
//   parser.loadDefaultArguments();

  // set up a gyro
  ArAnalogGyro gyro(&robot);
  //gyro.activate();
  // Parse arguments for the simple connector.
//   if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
//   {
//     ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
//     Aria::logOptions();
//     Aria::exit(1);
//   }


  // The laser object, will be used if we have one


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

  // Sonar, must be added to the robot, used by teleoperation and wander to
  // detect obstacles, and for localization if SONARNL
  ArSonarDevice sonarDev;

  // Add the sonar to the robot
  robot.addRangeDevice(&sonarDev);
  
  // Set up where we'll look for files
  char fileDir[1024];
  ArUtil::addDirectories(fileDir, sizeof(fileDir), "./", "maps");
  ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir);
  
  // Set up the map, this will look for files in the examples
  // directory (unless the file name starts with a /, \, or .
  // You can take out the 'fileDir' argument to look in the current directory
  // instead
  
  ArMap arMap(fileDir);
  // set it up to ignore empty file names (otherwise the parseFile
  // on the config will fail)
  arMap.setIgnoreEmptyFileName(true);
  
//********************************
//Localization
//********************************
  

  ArLocalizationManager locManager(&robot, &arMap);
  ArLog::log(ArLog::Normal, "Creating laser localization task");
  ArLocalizationTask locTask (&robot, &sick, &arMap);
  locManager.addLocalizationTask(&locTask);

  
//*******************************
//Path planning
//*******************************
  
  // Make the path task planning task
  ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap);
  G_PathPlanning = &pathTask;
//.........这里部分代码省略.........