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

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();
  ArRobot robot;

  ArSerialConnection serialConnection;
  ArTcpConnection tcpConnection;
    
  if (tcpConnection.open("localhost", 8101)) {
    robot.setDeviceConnection(&tcpConnection);
  } else {
    serialConnection.setPort("/dev/ttyUSB0");
    robot.setDeviceConnection(&serialConnection);
  }
  robot.blockingConnect();
   
  printf("Setting robot to run async\n");
  robot.runAsync(false);

  printf("Turning off sound\n");
  robot.comInt(ArCommands::SOUNDTOG, 0);

  printf("Enabling motors\n");
  robot.enableMotors();

  // add a set of actions that combine together to effect the wander behavior
  /*ArActionStallRecover recover;
  ArActionBumpers bumpers;
  ArActionAvoidFront avoidFrontNear("Avoid Front Near", 225, 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);*/

  printf("Locking\n");
  robot.lock();
  robot.setVel(100.0);
  robot.unlock();
  printf("Sleeping\n");
  ArUtil::sleep(3*1000);
  printf("Awake\n");

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

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

bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
    ROS_INFO("RosAria: Enable motors request.");
    robot->lock();
    if(robot->isEStopPressed())
        ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
    robot->enableMotors();
    robot->unlock();
	// todo could wait and see if motors do become enabled, and send a response with an error flag if not
    return true;
}

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

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

/*!
 * Sets up for path planning.
 *
 * @return true if successful.
 */
bool
Advanced::initializePathPlanningTask(void)
{
#ifdef SONARNL
  if(myRobot && mySonar && myMap)
    myPathPlanningTask = new ArPathPlanningTask(myRobot, mySonar,
                        myMap);
#else
  if(myRobot && mySick && mySonar && myMap)
    myPathPlanningTask = new ArPathPlanningTask(myRobot, mySick, mySonar,
                        myMap);
#endif

  if(!myPathPlanningTask){
    return false;
  }else{
    myRobot->lock();
    myRobot->enableMotors();
    myRobot->unlock();
    return true;
  }
}

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

  
  
  
  // add our logging to the config
//   ArLog::addToConfig(Aria::getConfig());

  // First open the server 
  if (!simpleOpener.open(&server, fileDir, 240))
  {
    if (simpleOpener.wasUserFileBad())
      ArLog::log(ArLog::Normal, "Bad user file");
    else
      ArLog::log(ArLog::Normal, "Could not open server port");
    exit(2);
  }

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

  
//-----------------------------------------------
//**************************
  // Set up a class that'll put the movement and gyro parameters into ArConfig
  ArRobotConfig robotConfig(&robot);
  robotConfig.addAnalogGyro(&gyro);
//*****************************
  
  
  robot.enableMotors();
  robot.clearDirectMotion();

  // if we are connected to a simulator, reset it to its start position
  robot.comInt(ArCommands::RESETSIMTOORIGIN, 1);
  robot.moveTo(ArPose(0,0,0));


  // Set up laser using connector (command line arguments, etc.)
   simpleConnector.setupLaser(&sick);

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

  // Try to connect the laser
  if (!sick.blockingConnect())
    ArLog::log(ArLog::Normal, "Warning: Couldn't connect to SICK laser, it won't be used");
  else
    ArLog::log(ArLog::Normal, "Connected to laser.");

//***************************************
  // Add additional range devices to the robot and path planning task.
  // IRs if the robot has them.
  robot.lock();
//   ArIRs irs;
//   robot.addRangeDevice(&irs);
//   pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT);
//******************************************

int main (int argc, char** argv) {
	
	Aria::init();
	Arnl::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &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;
	ArLog::log(ArLog::Normal, "OK");
	char fileDir[1024];
	ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), "examples");
	ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir);
	//		strcpy(fileDir, "columbia.map");
	ArLog::log(ArLog::Normal, "file Maps directory is: %s\n",fileDir);
	ArMap map(fileDir);
	map.readFile("columbia.map");
	// set it up to ignore empty file names (otherwise the parseFile
	// on the config will fail)
	map.setIgnoreEmptyFileName(true);



	robot.addRangeDevice(&sonarDev);
	// set home pose
	robot.moveTo(ArPose(0,0,0));

	ArPathPlanningTask pathPlan(&robot, &sonarDev, &map);
	ArActionGoto gotoPoseAction("goto");
//	gotoPoseAction.activate();
//	pathPlan.getPathPlanActionGroup()->addAction(&gotoPoseAction, 50);
//	pathPlan.getPathPlanAction()->activate();
	ArForbiddenRangeDevice forbidden(&map);
	robot.addRangeDevice(&forbidden);
	pathPlan.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);
//	pathPlan.planAndSetupAction(ArPose(0, 0, 0));
//	pathPlan.
/*
	if (pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true))
		ArLog::log(ArLog::Normal, "OK");
	else
		ArLog::log(ArLog::Normal, "FAILED");
*/
	// create functor
	ArGlobalFunctor1<ArPose> goalDone(&addGoalDone);// = new ArGlobalFunctor1(&addGoalDone);
	ArGlobalFunctor1<ArPose> goalFail(&addGoalFailed);
	ArGlobalFunctor1<ArPose> goalFinished(&addGoalFinished);
	ArGlobalFunctor1<ArPose> goalInterrupted(&addGoalInterrupted);

	// add functor
	pathPlan.addGoalDoneCB(&goalDone);
	pathPlan.addGoalFailedCB(&goalFail);
	pathPlan.addGoalFinishedCB(&goalFinished);
	pathPlan.addGoalInterruptedCB(&goalInterrupted);

	robot.runAsync(true);
	robot.enableMotors();
	pathPlan.runAsync();

//	ArPathPlanningTask::PathPlanningState state = pathPlan.getState();
//	while(!pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true));
	pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true);
	ArPathPlanningTask::PathPlanningState state = pathPlan.getState();
	char* s = "";
	switch(state)
	  {
	    case ArPathPlanningTask::NOT_INITIALIZED: {s = "NOT_INITIALIZED"; break;}
	    case ArPathPlanningTask::PLANNING_PATH: { s = "PLANNING_PATH";break;}
	    case ArPathPlanningTask::MOVING_TO_GOAL:{s = "MOVING_TO_GOAL";break;}
	    case ArPathPlanningTask::REACHED_GOAL: {s = "REACHED_GOAL";break;}
	    case ArPathPlanningTask::FAILED_PLAN: { s = "FAILED_PLAN";break;}
	    case ArPathPlanningTask::FAILED_MOVE: { s="FAILED_MOVE";break;}
	    case ArPathPlanningTask::ABORTED_PATHPLAN:{s = "ABORTED_PATHPLAN";break;}
//	    case ArPathPlanningTask::INVALID:
//	    default:
//	      return "UNKNOWN";
	  }
	ArLog::log(ArLog::Normal,s);
	robot.waitForRunExit();
//	pathPlan.
//	char* text = new char[512];
//	pathPlan.getStatusString(text, sizeof(text));
//	 printf("Planning status: %s.\n", text);
//	while(pathPlan.pathPlanToPose(ArPose(1000, 1000, 0), true, true));
	Aria::shutdown();
//	Arnl::s;
	Aria::exit(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, "teleopActionsExample: 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, "teleopActionsExample: Connected.");

  
  // limiter for close obstacles
  ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250);
  // limiter for far away obstacles
  ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400);
  // limiter that checks IR sensors (like Peoplebot has)
  ArActionLimiterTableSensor tableLimiter;
  // limiter so we don't bump things backwards
  ArActionLimiterBackwards backwardsLimiter;
  // the joydrive action
  ArActionJoydrive joydriveAct;
  // the keydrive action
  ArActionKeydrive keydriveAct;
  
  // sonar device, used by the limiter actions.
  ArSonarDevice sonar;



  printf("This program will allow you to use a joystick or keyboard to control the robot.\nYou can use the arrow keys to drive, and the spacebar to stop.\nFor joystick control press the trigger button and then drive.\nPress escape to exit.\n");

  // if we don't have a joystick, let 'em know
  if (!joydriveAct.joystickInited())
    printf("Do not have a joystick, only the arrow keys on the keyboard will work.\n");
  
  // add the sonar to the robot
  robot.addRangeDevice(&sonar);


  // set the robots maximum velocity (sonar don't work at all well if you're
  // going faster)
  robot.setAbsoluteMaxTransVel(400);

  // enable the motor
  robot.enableMotors();

  // Add the actions, with the limiters as highest priority, then the teleop.
  // actions.  This will keep the teleop. actions from being able to drive too 
  // fast and hit something
  robot.addAction(&tableLimiter, 100);
  robot.addAction(&limiter, 95);
  robot.addAction(&limiterFar, 90);
  robot.addAction(&backwardsLimiter, 85);
  robot.addAction(&joydriveAct, 50);
  robot.addAction(&keydriveAct, 45);

  // Configure the joydrive action so it will let the lower priority actions
  // (i.e. keydriveAct) request motion if the joystick button is
  // not pressed.
  joydriveAct.setStopIfNoButtonPressed(false);

  
  // run the robot, true means that the run will exit if connection lost
  robot.run(true);
  
  Aria::exit(0);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "terabot_arm_hardware_interface");
    ros::NodeHandle node;
    std::cout << "argc= "<<argc<<" argv= "<<argv<<std::endl;
    hardware_interface::JointStateInterface jnt_state_interface_;
    hardware_interface::PositionJointInterface jnt_pos_interface_;

    //**********************************************************
    //**********************************************************
     Aria::init();
      static float defaultJointSpeed = 15;
      ArLog::init(ArLog::StdErr, ArLog::Normal);
      ArArgumentParser parser(&argc, argv);
      parser.loadDefaultArguments();
      ArRobot robot;
      ArTerabotArm arm(&robot);
      

      
      
      ArRobotConnector robotConnector(&parser, &robot);

      if(!robotConnector.connectRobot())
      {
        ArLog::log(ArLog::Terse, "terabotArm: 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, "terabotArm: Connected to mobile robot.");


      if(!arm.open())
      {
          ArLog::log(ArLog::Terse, "terabotArm: Error opening serial connection to arm");
          Aria::exit(1);
      }


      robot.runAsync(true);
      
      arm.powerOn();
      arm.reset();
      arm.enable();
      arm.setAllJointSpeeds(defaultJointSpeed);
     
      ArUtil::sleep(5000);
      
      robot.lock();
  
      robot.enableMotors(); 
      robot.unlock();
    
    
    
      TerabotArmInterface robot1(&robot, &arm, node);   
    
    //**********************************************************
    //**********************************************************
    
  //  TerabotArmInterface robot1(&robot, &arm);
    std::cout << "after creating the object"<<std::endl;
    robot1.init(jnt_state_interface_, jnt_pos_interface_);
    controller_manager::ControllerManager cm(&robot1, node);
    


    ros::AsyncSpinner spinner(4);
    spinner.start();

    ros::Time previous=ros::Time::now();

    ros::Rate rate(10.0);
    while (ros::ok())
    {
        ros::Duration period;
        robot1.readHW();
        ros::Time now=ros::Time::now();
        period=now-previous;
        //std::cout << "period:"<<period<<std::endl;
        cm.update(now, period);
        robot1.writeHW();
        rate.sleep();

    }

    spinner.stop();

    return 0;
}

int __cdecl _tmain (int argc, char** argv)
{

	//------------ I N I C I O   M A I N    D E L   P R O G R A M A   D E L    R O B O T-----------//

	  //inicializaion de variables
	  Aria::init();
	  ArArgumentParser parser(&argc, argv);
	  parser.loadDefaultArguments();
	  ArSimpleConnector simpleConnector(&parser);
	  ArRobot robot;
	  ArSonarDevice sonar;
	  ArAnalogGyro gyro(&robot);
	  robot.addRangeDevice(&sonar);
	  ActionGos go(500, 350);	  
	  robot.addAction(&go, 48);
	  ActionTurns turn(400, 110);
	  robot.addAction(&turn, 49);
	  ActionTurns turn2(400, 110);
	  robot.addAction(&turn2, 49);

	  // presionar tecla escape para salir del programa
	  ArKeyHandler keyHandler;
	  Aria::setKeyHandler(&keyHandler);
	  robot.attachKeyHandler(&keyHandler);
	  printf("Presionar ESC para salir\n");

	  // uso de sonares para evitar colisiones con las paredes u 
	  // obstaculos grandes, mayores a 8cm de alto
	  ArActionLimiterForwards limiterAction("limitador velocidad cerca", 300, 600, 250);
	  ArActionLimiterForwards limiterFarAction("limitador velocidad lejos", 300, 1100, 400);
	  ArActionLimiterTableSensor tableLimiterAction;
	  robot.addAction(&tableLimiterAction, 100);
	  robot.addAction(&limiterAction, 95);
	  robot.addAction(&limiterFarAction, 90);


	  // Inicializon la funcion de goto
	  ArActionGoto gotoPoseAction("goto");
	  robot.addAction(&gotoPoseAction, 50);
	  
	  // Finaliza el goto si es que no hace nada
	  ArActionStop stopAction("stop");
	  robot.addAction(&stopAction, 40);

	  // Parser del CLI
	  if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
	  {    
		Aria::logOptions();
		exit(1);
	  }
	  
	  // Conexion del robot
	  if (!simpleConnector.connectRobot(&robot))
	  {
		printf("Could not connect to robot... exiting\n");
		Aria::exit(1);
	  }
	  robot.runAsync(true);

	  // enciende motores, apaga sonidos
	  robot.enableMotors();
	  robot.comInt(ArCommands::SOUNDTOG, 0);

	  // Imprimo algunos datos del robot como posicion velocidad y bateria
		robot.lock();
		ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
			robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
		robot.unlock();

	  const int duration = 100000; //msec
	  ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000);

	  // ============================ INICIO CONFIG COM =================================//
	    CSerial serial;
		LONG    lLastError = ERROR_SUCCESS;

		// Trata de abrir el com seleccionado
		lLastError = serial.Open(_T("COM3"),0,0,false);
		if (lLastError != ERROR_SUCCESS)
			return ::ShowError(serial.GetLastError(), _T("Imposible abrir el COM"));

		// Inicia el puerto serial (9600,8N1)
		lLastError = serial.Setup(CSerial::EBaud9600,CSerial::EData8,CSerial::EParNone,CSerial::EStop1);
		if (lLastError != ERROR_SUCCESS)
			return ::ShowError(serial.GetLastError(), _T("Imposible setear la config del COM"));

		// Register only for the receive event
		lLastError = serial.SetMask(CSerial::EEventBreak |
									CSerial::EEventCTS   |
									CSerial::EEventDSR   |
									CSerial::EEventError |
									CSerial::EEventRing  |
									CSerial::EEventRLSD  |
									CSerial::EEventRecv);
		if (lLastError != ERROR_SUCCESS)
			return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port event mask"));

		// Use 'non-blocking' reads, because we don't know how many bytes
		// will be received. This is normally the most convenient mode
//.........这里部分代码省略.........

int main ( int argc, char *argv[] ){
//cout << "running....\n";
try{
	// Create the socket
	ServerSocket server ( 30000 );

	Aria::init();
	Arnl::init();

	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArSonarDevice sonar;
	ArSimpleConnector simpleConnector(&parser);

	// Our server for mobile eyes
	ArServerBase moServer;

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

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

	//Add the sonar to the robot
	robot.addRangeDevice(&sonar);

  	// Look for map in the current directory
  	ArMap arMap;
  	// set it up to ignore empty file names (otherwise the parseFile
  	// on the config will fail)
  	arMap.setIgnoreEmptyFileName (true);

	// First open the server 
	if (!simpleOpener.open(&moServer)){
		if (simpleOpener.wasUserFileBad())
    			ArLog::log(ArLog::Normal, "Bad user file");
		else
    			ArLog::log(ArLog::Normal, "Could not open server port");
  		exit(2);
	}

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

	// Create the localization task (it will start its own thread here)
	ArSonarLocalizationTask locTask(&robot, &sonar, &arMap);

	ArLocalizationManager locManager(&robot, &arMap);

	ArLog::log(ArLog::Normal, "Creating sonar localization task");
	locManager.addLocalizationTask(&locTask);




	// Set the initial pose to the robot's "Home" position from the map, or
	// (0,0,0) if none, then let the localization thread take over.
	locTask.localizeRobotAtHomeNonBlocking();

	//Create the path planning task
	ArPathPlanningTask pathTask(&robot,&sonar,&arMap);

	ArLog::log(ArLog::Normal, "Robot Server: Connected.");

	robot.enableMotors();
	robot.clearDirectMotion();

	// Start the robot processing cycle running in the background.
	// True parameter means that if the connection is lost, then the
	// run loop ends.
	robot.runAsync(true);

	// Read in parameter files.
  	Aria::getConfig ()->useArgumentParser (&parser);
  	if (!Aria::getConfig ()->parseFile (Arnl::getTypicalParamFileName ())){
		ArLog::log (ArLog::Normal, "Trouble loading configuration file, exiting");
		Aria::exit (5);
	}

	//Create the three states
	robot.lock();
	Follow follow = Follow(&robot,&sonar);
	GoTo goTo(&robot,&pathTask,&arMap);
	Search s(&robot,&sonar);

	// Bumpers.
  	ArBumpers bumpers;
  	robot.addRangeDevice(&bumpers);
  	pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT);

  	// Forbidden regions from the map
  	ArForbiddenRangeDevice forbidden(&arMap);
  	robot.addRangeDevice(&forbidden);
//.........这里部分代码省略.........