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

void SetupRobot(void)
{
	puts("attempting to connect to robot");
	RobotConnectoin.setPort("COM8");
	RobotConnectoin.setBaud(9600);
	robot.setDeviceConnection(&RobotConnectoin);
	if(!robot.blockingConnect()){puts("not connected to robot");Aria::shutdown();}
	robot.addRangeDevice(&sonarDev);
	robot.addRangeDevice(&bumpers);
	robot.enableMotors();
	robot.enableSonar();
	robot.requestEncoderPackets();
	robot.setCycleChained(false);
//	robot.setRotVelMax(robot.getRotVelMax());
}

int main(int argc, char** argv)
{
  Aria::init();
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArRobot robot;
  ArSonarDevice sonar;

  // Connect to the robot, get some initial data from it such as type and name,
  // and then load parameter files for this robot.
  ArRobotConnector robotConnector(&parser, &robot);
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "actionExample: 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, "actionExample: Connected to robot.");

  // Create instances of the actions defined above, plus ArActionStallRecover, 
  // a predefined action from Aria.
  ActionGo go(500, 350);
  ActionTurn turn(400, 10);
  ArActionStallRecover recover;

    
  // Add the range device to the robot. You should add all the range 
  // devices and such before you add actions
  robot.addRangeDevice(&sonar);

 
  // Add our actions in order. The second argument is the priority, 
  // with higher priority actions going first, and possibly pre-empting lower
  // priority actions.
  robot.addAction(&recover, 100);
  robot.addAction(&go, 50);
  robot.addAction(&turn, 49);

  // Enable the motors, disable amigobot sounds
  robot.enableMotors();

  // Run the robot processing cycle.
  // 'true' means to return if it loses connection,
  // after which we exit the program.
  robot.run(true);
  
  Aria::exit(0);
}

int main (int argc, char** argv) {
	
	Aria::init();
	Arnl::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &robot);
	//ArAnalogGyro gyro = new 
	ArAnalogGyro gyro(&robot);
	if (!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(parser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}
	
	ArSonarDevice sonarDev;
	robot.addRangeDevice(&sonarDev);
	
	robot.runAsync(true);
	
	ArMap map("office.map");
	// set it up to ignore empty file names (otherwise if a configuration omits
	// the map file, the whole configuration change will fail)
	map.setIgnoreEmptyFileName(true);
	// ignore the case, so that if someone is using MobileEyes or
	// MobilePlanner from Windows and changes the case on a map name,
	// it will still work.
	map.setIgnoreCase(true);
	
	ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
	
	locTask.localizeRobotAtHomeBlocking();
	
	robot.runAsync(true);
	robot.enableMotors();
	//robot.lock();
	robot.setVel(200);
	//robot.unlock();
	ArPose pose;
	locTask.forceUpdatePose(pose);
	while(true) {
	//while (robot.blockingConnect()){
		//robot.lock();
		//ArPose pose  = robot.getPose();
		//pose.setX(100);
		//robot.moveTo(pose);
		//t = robot.getLastOdometryTime();
		//int a = interp.getPose(t, &pose);
		ArLog::log(ArLog::Normal, "x = %f \t y = %f\n", pose.getX(), pose.getY());
		//robot.unlock();
	} 
}

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

  ArSimpleConnector conn(&argc, argv);
  ArRobot robot;
  ArSonarDevice sonar;

  // Create instances of the actions defined above, plus ArActionStallRecover, 
  // a predefined action from Aria.
  ActionGo go(500, 350);
  ActionTurn turn(400, 10);
  ArActionStallRecover recover;

    
  // Parse all command-line arguments
  if(!Aria::parseArgs())
  {
    Aria::logOptions();
    return 1;
  }
  
  // Connect to the robot
  if(!conn.connectRobot(&robot))
  {
    ArLog::log(ArLog::Terse, "actionExample: Could not connect to robot! Exiting.");
    return 2;
  }

  // Add the range device to the robot. You should add all the range 
  // devices and such before you add actions
  robot.addRangeDevice(&sonar);

 
  // Add our actions in order. The second argument is the priority, 
  // with higher priority actions going first, and possibly pre-empting lower
  // priority actions.
  robot.addAction(&recover, 100);
  robot.addAction(&go, 50);
  robot.addAction(&turn, 49);

  // Enable the motors, disable amigobot sounds
  robot.enableMotors();

  // Run the robot processing cycle.
  // 'true' means to return if it loses connection,
  // after which we exit the program.
  robot.run(true);
  
  Aria::shutdown();
  return 0;
}

int main(int argc, char **argv){
	Aria::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	ArSimpleConnector connector(& parser);

	parser.loadDefaultArguments();
	Aria::logOptions();
	if (!connector.parseArgs()){
		cout << "Unknown settings\n";
		Aria::exit(0);
		exit(1);
	}

	if (!connector.connectRobot(&robot)){
		cout << "Unable to connect\n";
		Aria::exit(0);
		exit(1);
	}

	robot.runAsync(true);
	robot.lock();
	robot.comInt(ArCommands::ENABLE, 1);
	robot.unlock();



	ArSonarDevice sonar;
	robot.addRangeDevice(&sonar);

	G_id = 0;
	G_SONAR_FD = fopen("../sensors/sonars","w");
	G_pose_fd  = fopen("../sensors/pose","w");
	int numSonar = robot.getNumSonar();
	while(1){
		readPosition(robot);
		readSonars(robot, 8);
		setMotors(robot);
		usleep(20000);
	}

	fclose(G_SONAR_FD);
	fclose(G_pose_fd);
	Aria::exit(0);
}

int main (int argc, char** argv) {
	
	Aria::init();
	Arnl::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &robot);
	//ArAnalogGyro gyro = new 
	ArAnalogGyro gyro(&robot);
	if (!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(parser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}
	
	ArSonarDevice sonarDev;
	

//	robot.runAsync(true);
	
//	ArMap map("office.map");
	// set it up to ignore empty file names (otherwise if a configuration omits
	// the map file, the whole configuration change will fail)
//	map.setIgnoreEmptyFileName(true);
	// ignore the case, so that if someone is using MobileEyes or
	// MobilePlanner from Windows and changes the case on a map name,
	// it will still work.
//	map.setIgnoreCase(true);


	robot.runAsync(true);
	robot.enableMotors();
	robot.moveTo(ArPose(0,0,0));
	//robot.lock();
	robot.comInt(ArCommands::ENABLE, 1);
	robot.addRangeDevice(&sonarDev);
	//robot.unlock();
//	ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
<<<<<<< HEAD

int main(void)
{
  ArTcpConnection con;

  ArRobot robot;
  ArSonarDevice sonar;

  int ret;
  std::string str;
  ArActionStallRecover recover;
  ArActionConstantVelocity constantVelocity("Constant Velocity", 400);

  Aria::init();

  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }
  
  robot.addRangeDevice(&sonar);
  robot.setDeviceConnection(&con);
  if (!robot.blockingConnect())
  {
    printf("Could not connect to robot... exiting\n");
    Aria::shutdown();
    return 1;
  }

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

  robot.addAction(&recover, 100);
  robot.addAction(&constantVelocity, 25);
  robot.run(true);
  
  Aria::shutdown();
  return 0;
}

int RosAriaNode::Setup()
{
	ArArgumentBuilder *args;

	args = new ArArgumentBuilder();
	args->add("-rp"); //pass robot's serial port to Aria
	args->add(serial_port.c_str());
	conn = new ArSimpleConnector(args);


	robot = new ArRobot();
	robot->setCycleTime(1);

	ArLog::init(ArLog::File, ArLog::Verbose, "aria.log", true);

	//parse all command-line arguments
	if (!Aria::parseArgs())
	{
		Aria::logOptions();
		return 1;
	}

	// Connect to the robot
	if (!conn->connectRobot(robot)) {
		ArLog::log(ArLog::Terse, "rotate: Could not connect to robot! Exiting.");
		return 1;
	}

	//Sonar sensor
	sonar.setMaxRange(Max_Range);
	robot->addRangeDevice(&sonar);
	robot->enableSonar();

	// Enable the motors
	robot->enableMotors();

	robot->runAsync(true);

	return 0;
}

int main (int argc, char** argv) {
	
	Aria::init();
	Arnl::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &robot);
	//ArAnalogGyro gyro = new 
//	ArAnalogGyro gyro(&robot);
	if (!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(parser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}
	
	ArSonarDevice sonarDev;
	

//	robot.runAsync(true);
	ArLog::log(ArLog::Normal, "OK");
	ArMap map("office.map");
	// set it up to ignore empty file names (otherwise if a configuration omits
	// the map file, the whole configuration change will fail)
//	map.setIgnoreEmptyFileName(true);
	// ignore the case, so that if someone is using MobileEyes or
	// MobilePlanner from Windows and changes the case on a map name,
	// it will still work.
//	map.setIgnoreCase(true);


	robot.runAsync(true);

//	robot.lock();
//	robot.comInt(ArCommands::ENABLE, 1);
	robot.addRangeDevice(&sonarDev);
//	ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
//	ArActionAvoidFront avoidFront("avoid front obstacles");
//	ArActionGoto gotoPoseAction("goto");
//	robot.addAction(&gotoPoseAction, 50);
//	robot.addAction(&avoidFront, 60);
//	robot.moveTo(ArPose(0,0,0));
	ArPathPlanningTask pathPlan(&robot, &sonarDev, &map);
//	pathPlan.getRun
//	locTask.localizeRobotAtHomeBlocking();
	ArGlobalFunctor1<ArPose> goalDone(&addGoalDone);// = new ArGlobalFunctor1(&addGoalDone);
	ArGlobalFunctor1<ArPose> goalFail(&addGoalFailed);
	ArGlobalFunctor1<ArPose> goalFinished(&addGoalFinished);
	ArGlobalFunctor1<ArPose> goalInterrupted(&addGoalInterrupted);
	pathPlan.addGoalDoneCB(&goalDone);
	pathPlan.addGoalFailedCB(&goalFail);
	pathPlan.addGoalFinishedCB(&goalFinished);
	pathPlan.addGoalInterruptedCB(&goalInterrupted);

//	ro
//	ArActionPlanAndMoveToGoal gotoGoal (200, 10, pathPlan, NULL, &sonarDev);

//	pathPlan.runAsync();
	robot.enableMotors();
//	while(!pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true));
	pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true);
//	pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true)
	pathPlan.startPathPlanToLocalPose(true);
//	pathPlan.getPathPlanActionGroup()->activate();
//	pathPlan.getCurrentGoal()
//	pathPlan.is
//	robot.unlock();
	/*
	gotoPoseAction.setGoal(ArPose(10000, 15000, 0));
	while (!gotoPoseAction.haveAchievedGoal()) {
		robot.lock();
		printf ("x = %.2f, y = %.2f", robot.getX(), robot.getY());
		robot.unlock();
	}
	*/
//	robot.lock();
//	robot.setVel(200);
//	robot.unlock();
	while (pathPlan.endPathPlanToLocalPose(true));
		//ArUtil::sleep(1000);

//	ArPose pose;
//	locTask.forceUpdatePose(pose);
	
	/*
	while(true) {
	//while (robot.blockingConnect()){
		//robot.lock();
		//ArPose pose  = robot.getPose();
		//pose.setX(100);
		//robot.moveTo(pose);
		//t = robot.getLastOdometryTime();
		//int a = interp.getPose(t, &pose);
		ArLog::log(ArLog::Normal, "x = %f \t y = %f\n", pose.getX(), pose.getY());
		//robot.unlock();
//.........这里部分代码省略.........

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

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

  // set up our parser
  ArArgumentParser parser(&argc, argv);

  // load the default arguments 
  parser.loadDefaultArguments();

  // robot
  ArRobot robot;
  // set up our simple connector
  ArRobotConnector robotConnector(&parser, &robot);


  // add a gyro, it'll see if it should attach to the robot or not
  ArAnalogGyro gyro(&robot);


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

  ArDataLogger dataLogger(&robot, "dataLog.txt");
  dataLogger.addToConfig(Aria::getConfig());
  
  // our base server object
  ArServerBase server;

  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
  ArServerSimpleOpener simpleOpener(&parser);


  ArClientSwitchManager clientSwitchManager(&server, &parser);

  // parse the command line... fail and print the help if the parsing fails
  // or if the help was requested
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {    
    Aria::logOptions();
    Aria::exit(1);
  }

  // Set up where we'll look for files such as user/password 
  char fileDir[1024];
  ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), 
			 "ArNetworking/examples");

  // first open the server up
  if (!simpleOpener.open(&server, fileDir, 240))
  {
    if (simpleOpener.wasUserFileBad())
      printf("Bad user/password/permissions file\n");
    else
      printf("Could not open server port\n");
    exit(1);
  }

  // Range devices:
 
 
  ArSonarDevice sonarDev;
  robot.addRangeDevice(&sonarDev);

  ArIRs irs;
  robot.addRangeDevice(&irs);

  ArBumpers bumpers;
  robot.addRangeDevice(&bumpers);

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

  // modes for controlling robot movement
  ArServerModeStop modeStop(&server, &robot);
  ArServerModeRatioDrive modeRatioDrive(&server, &robot);  
  ArServerModeWander modeWander(&server, &robot);
  modeStop.addAsDefaultMode();
  modeStop.activate();

  // set up the simple commands
  ArServerHandlerCommands commands(&server);
  ArServerSimpleComUC uCCommands(&commands, &robot);  // send commands directly to microcontroller
  ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // control debug logging
  ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro); // configure gyro
  ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // control more debug logging
  ArServerSimpleServerCommands serverCommands(&commands, &server); // control ArNetworking debug logging
  ArServerSimpleLogRobotDebugPackets logRobotDebugPackets(&commands, &robot, ".");  // debugging tool

  // ArServerModeDrive is an older drive mode. ArServerModeRatioDrive is newer and generally performs better,
  // but you can use this for old clients if neccesary.
//.........这里部分代码省略.........

int main(int argc, char *argv[])
{
    // Initialize location of Aria, Arnl and their args.
    Aria::init();
    Arnl::init();


    // The robot object
    ArRobot robot;

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

    // Read data_index if exists
    int data_index;
    bool exist_data_index;
    parser.checkParameterArgumentInteger("dataIndex",&data_index,&exist_data_index);


    // Load default arguments for this computer (from /etc/Aria.args, environment
    // variables, and other places)
    parser.loadDefaultArguments();


    // Object that connects to the robot or simulator using program options
    ArRobotConnector robotConnector(&parser, &robot);


    // set up a gyro
    ArAnalogGyro gyro(&robot);

    ArLog::init(ArLog::File,ArLog::Normal,"run.log",false,true,true);

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

    // Collision avoidance actions at higher priority
    ArActionAvoidFront avoidAction("avoid",200);
    ArActionLimiterForwards limiterAction("speed limiter near", 150, 500, 150);
    ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
    ArActionLimiterTableSensor tableLimiterAction;
    //robot.addAction(&tableLimiterAction, 100);
    //robot.addAction(&avoidAction,100);
    //robot.addAction(&limiterAction, 95);
    //robot.addAction(&limiterFarAction, 90);

    // Goto action at lower priority
    ArActionGoto gotoPoseAction("goto");
    //robot.addAction(&gotoPoseAction, 50);
    gotoPoseAction.setCloseDist(750);

    // Stop action at lower priority, so the robot stops if it has no goal
    ArActionStop stopAction("stop");
    //robot.addAction(&stopAction, 40);




    // Connect the robot
    if (!robotConnector.connectRobot())
    {
        ArLog::log(ArLog::Normal, "Could not connect to robot... exiting");
        Aria::exit(3);
    }

    if(!robot.isConnected())
    {
        ArLog::log(ArLog::Terse, "Internal error: robot connector succeeded but ArRobot::isConnected() is false!");
    }




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


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

    // Start the robot thread.
    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
    // connected by this call so when you enter laser mode, you
    // can then interactively choose which laser to use from that list of all
    // lasers mentioned in robot parameters and on command line. Normally,
//.........这里部分代码省略.........

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

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)
{
  Aria::init();
  ArRobot robot;
  ArServerBase server;

  ArArgumentParser parser(&argc, argv);
  ArSimpleConnector simpleConnector(&parser);
  ArServerSimpleOpener simpleOpener(&parser);


  // parse the command line... fail and print the help if the parsing fails
  // or if help was requested
  parser.loadDefaultArguments();
  if (!simpleConnector.parseArgs() || !simpleOpener.parseArgs() || 
      !parser.checkHelpAndWarnUnparsed())
  {    
    simpleConnector.logOptions();
    simpleOpener.logOptions();
    exit(1);
  }

  // Set up where we'll look for files such as config file, user/password file,
  // etc.
  char fileDir[1024];
  ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), 
			 "ArNetworking/examples");

  // first open the server up
  if (!simpleOpener.open(&server, fileDir, 240))
  {
    if (simpleOpener.wasUserFileBad())
      printf("Error: Bad user/password/permissions file.\n");
    else
      printf("Error: Could not open server port. Use -help to see options.\n");
    exit(1);
  }


  // Devices
  ArAnalogGyro gyro(&robot);

  ArSonarDevice sonarDev;
  robot.addRangeDevice(&sonarDev);

  ArIRs irs;
  robot.addRangeDevice(&irs);

  ArBumpers bumpers;
  robot.addRangeDevice(&bumpers);

  ArSick sick(361, 180);
  robot.addRangeDevice(&sick);  
  

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

int main(int argc, char **argv)
{
  // Initialize Aria and Arnl global information
  Aria::init();
  Arnl::init();


  // The robot object
  ArRobot robot;

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

  // Set up our simpleConnector, to connect to the robot and laser
  //ArSimpleConnector simpleConnector(&parser);
  ArRobotConnector robotConnector(&parser, &robot);

  // Connect to the robot
  if (!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Normal, "Error: Could not connect to robot... exiting");
    Aria::exit(3);
  }



  // Set up where we'll look for files. Arnl::init() set Aria's default
  // directory to Arnl's default directory; addDirectories() appends this
  // "examples" directory.
  char fileDir[1024];
  ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), 
			 "examples");
  
  
  // To direct log messages to a file, or to change the log level, use these  calls:
  //ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true);
  //ArLog::init(ArLog::File, ArLog::Verbose);
 
  // Add a section to the configuration to change ArLog parameters
  ArLog::addToConfig(Aria::getConfig());

  // set up a gyro (if the robot is older and its firmware does not
  // automatically incorporate gyro corrections, then this object will do it)
  ArAnalogGyro gyro(&robot);

  // Our networking server
  ArServerBase server;
  

  // Set up our simpleOpener, used to set up the networking server
  ArServerSimpleOpener simpleOpener(&parser);

  // the laser connector
  ArLaserConnector laserConnector(&parser, &robot, &robotConnector);

  // Tell the laser connector to always connect the first laser since
  // this program always requires a laser.
  parser.addDefaultArgument("-connectLaser");
  
  // Load default arguments for this computer (from /etc/Aria.args, environment
  // variables, and other places)
  parser.loadDefaultArguments();

  // Parse arguments 
  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
  {
    logOptions(argv[0]);
    Aria::exit(1);
  }
  

  // This causes Aria::exit(9) to be called if the robot unexpectedly
  // disconnects
  ArGlobalFunctor1<int> shutdownFunctor(&Aria::exit, 9);
  robot.addDisconnectOnErrorCB(&shutdownFunctor);


  // Create an ArSonarDevice object (ArRangeDevice subclass) and 
  // connect it to the robot.
  ArSonarDevice sonarDev;
  robot.addRangeDevice(&sonarDev);



  // This object will allow robot's movement parameters to be changed through
  // a Robot Configuration section in the ArConfig global configuration facility.
  ArRobotConfig robotConfig(&robot);

  // Include gyro configuration options in the robot configuration section.
  robotConfig.addAnalogGyro(&gyro);

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

  // connect the laser(s) if it was requested, this adds them to the
  // robot too, and starts them running in their own threads
  if (!laserConnector.connectLasers())
  {
    ArLog::log(ArLog::Normal, "Could not connect to all lasers... exiting\n");
//.........这里部分代码省略.........