void laserRequest_and_odom(ArServerClient *client, ArNetPacket *packet)
{ 
  robot.lock();
  ArNetPacket sending;
  sending.empty();
  ArLaser* laser = robot.findLaser(1);
  if(!laser){
      printf("Could not connect to Laser... exiting\n");
      Aria::exit(1);}	
  laser->lockDevice();
  const std::list<ArSensorReading*> *sensorReadings = laser->getRawReadings(); // see ArRangeDevice interface doc
  sending.byte4ToBuf((ArTypes::Byte4)(sensorReadings->size()));
  for (std::list<ArSensorReading*>::const_iterator it2= sensorReadings->begin(); it2 != sensorReadings->end(); ++it2){
	ArSensorReading* laserRead =*it2;
        sending.byte4ToBuf((ArTypes::Byte4)(laserRead->getRange()));
	//printf("%i,%i:",laserRead->getRange(),laserRead->getIgnoreThisReading());
  }
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getX()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getY()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getTh()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getVel()));
  sending.byte4ToBuf((ArTypes::Byte4)(robot.getRotVel()));
  //printf("%1f,%1f,%1f\n",robot.getX(),robot.getY(),robot.getTh());
  laser->unlockDevice();
  robot.unlock();
  sending.finalizePacket();
  //sending.printHex();
  client->sendPacketTcp(&sending);
}
void Joydrive::drive(void)
{
  int trans, rot;

  // print out some data about the robot
  printf("\rx %6.1f  y %6.1f  tth  %6.1f vel %7.1f mpacs %3d   ", 
	 myRobot->getX(), myRobot->getY(), myRobot->getTh(), 
	 myRobot->getVel(), myRobot->getMotorPacCount());
  fflush(stdout);

  // see if a joystick butotn is pushed, if so drive
  if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
				    myJoyHandler.getButton(2)))
  {
    // get the values out of the joystick handler
    myJoyHandler.getAdjusted(&rot, &trans);
    // drive the robot
    myRobot->setVel(trans);
    myRobot->setRotVel(-rot);
  }
  // if a button isn't pushed, stop the robot
  else
  {
    myRobot->setVel(0);
    myRobot->setRotVel(0);
  }
}
// this is the function called in the new thread
void *Joydrive::runThread(void *arg)
{
  threadStarted();

  int trans, rot;

  // only run while running, ie play nice and pay attention to the thread 
  //being shutdown
  while (myRunning)
  {
    // lock the robot before touching it
    myRobot->lock();
    if (!myRobot->isConnected())
    {
      myRobot->unlock();
      break;
    }
    // print out some information about the robot
    printf("\rx %6.1f  y %6.1f  tth  %6.1f vel %7.1f mpacs %3d   ", 
	   myRobot->getX(), myRobot->getY(), myRobot->getTh(), 
	   myRobot->getVel(), myRobot->getMotorPacCount());
    fflush(stdout);
    // if one of the joystick buttons is pushed, drive the robot
    if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
					myJoyHandler.getButton(2)))
    {
      // get out the values from the joystick
      myJoyHandler.getAdjusted(&rot, &trans);
      // drive the robot
      myRobot->setVel(trans);
      myRobot->setRotVel(-rot);
    }
    // if no buttons are pushed stop the robot
    else
    {
      myRobot->setVel(0);
      myRobot->setRotVel(0);
    }
    // unlock the robot, so everything else can run
    myRobot->unlock();
    // now take a little nap
    ArUtil::sleep(50);
  }
  // return out here, means the thread is done
  return NULL;
}
Exemple #4
0
int main(int argc, char **argv) 
{
  std::string str;
  int ret;
  int dist;
  ArTime start;
  ArPose startPose;
  bool vel2 = false;

  // connection to the robot
  ArSerialConnection con;
  // the robot
  ArRobot robot;
  // the connection handler from above
  ConnHandler ch(&robot);
  
  // init area with a dedicated signal handling thread
  Aria::init(Aria::SIGHANDLE_THREAD);

  if (argc != 2 || (dist = atoi(argv[1])) == 0)
    {
      printf("Usage: %s <distInMM>\n", argv[0]);
      exit(0);
    }
  if (dist < 1000)
    {
      printf("You must go at least a meter\n");
      exit(0);
    }
  // open the connection with the defaults, exit if failed
  if ((ret = con.open()) != 0)
  {
    str = con.getOpenMessage(ret);
    printf("Open failed: %s\n", str.c_str());
    Aria::shutdown();
    return 1;
  }

  // set the robots connection
  robot.setDeviceConnection(&con);
  // try to connect, if we fail, the connection handler should bail
  if (!robot.blockingConnect())
  {
    // this should have been taken care of by the connection handler
    // but just in case
    printf(
    "asyncConnect failed because robot is not running in its own thread.\n");
    Aria::shutdown();
    return 1;
  }
  // run the robot in its own thread, so it gets and processes packets and such
  robot.runAsync(false);

  // just a big long set of printfs, direct motion commands and sleeps,
  // it should be self-explanatory

  robot.lock();

  /*
  robot.setAbsoluteMaxTransVel(2000);
  robot.setTransVelMax(2000);
  robot.setTransAccel(1000);
  robot.setTransDecel(1000);
  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

  */
  printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist);
  if (vel2)
    robot.setVel2(2200, 2200);
  else
    robot.setVel(2200);
  robot.unlock();
  start.setToNow();
  startPose = robot.getPose();
  while (1)
  {
    robot.lock();
    printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f",
	   robot.getVel(), robot.getX(), robot.getY(), 
	   startPose.findDistanceTo(robot.getPose()),
	   robot.getTh());
    if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000)
    {
      printf("\nFinished distance\n");
      robot.setVel(0);
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 10000)
    {
      printf("\nDistance timed out\n");
      robot.setVel(0);
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  
  if (vel2)
    robot.setVel2(0, 0);
  else
    robot.setVel(0);
  start.setToNow();
  while (1)
    {
      robot.lock();
      if (vel2)
	robot.setVel2(0, 0);
      else
	robot.setVel(0);
      if (fabs(robot.getVel()) < 20)
	{
	  printf("Stopped\n");
	  robot.unlock();
	  break;
	}
      if (start.mSecSince() > 2000)
	{
	  printf("\nStop timed out\n");
	  robot.unlock();
	  break;
	}
      robot.unlock();
      ArUtil::sleep(50);
    }
  robot.lock();
  robot.disconnect();
  robot.unlock();
  // shutdown and ge tout
  Aria::shutdown();
  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
		// (and also the default mode for reading data).
		lLastError = serial.SetupReadTimeouts(CSerial::EReadTimeoutNonblocking);
		if (lLastError != ERROR_SUCCESS)
			return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port read timeout."));
		// ============================ FIN CONFIG COM =================================//

	  bool first = true;
	  int goalNum = 0;
	  int color = 3;
	  ArTime start;
	  start.setToNow();
	  while (Aria::getRunning()) 
	  {
		robot.lock();

		// inicia el primer punto 
		if (first || gotoPoseAction.haveAchievedGoal())
		{
		  first = false;
		  
		  goalNum++; //cambia de 0 a 1 el contador
		  printf("El contador esta en: --> %d <---\n",goalNum);
		  if (goalNum > 20)
			goalNum = 1;

		  //comienza la secuencia de puntos
		  if (goalNum == 1)
		  {
			gotoPoseAction.setGoal(ArPose(1150, 0));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			// 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();
			// Create the sound queue.
			ArSoundsQueue soundQueue;
			// Run the sound queue in a new thread
			soundQueue.runAsync();
			std::vector<const char*> filenames;
			filenames.push_back("sound-r2a.wav");
			soundQueue.play(filenames[0]);
		  }
		  else if (goalNum == 2)
		  {
			  printf("Gira 90 grados izquierda\n");
			  robot.unlock();
			  turn.myActivate = 1;
			  turn.myDirection = 1;
			  turn.activate();
			  ArUtil::sleep(1000);
			  turn.deactivate();
			  turn.myActivate = 0;
			  turn.myDirection = 0;
			  robot.lock();
		  }
		  else if (goalNum == 3)
		  {
			gotoPoseAction.setGoal(ArPose(1150, 2670));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			// 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();
		  }
		  else if (goalNum == 4)
		  {
			  printf("Gira 90 grados izquierda\n");
			  robot.unlock();
			  turn2.myActivate = 1;
			  turn2.myDirection = 1;
			  turn2.activate();
			  ArUtil::sleep(1000);
			  turn2.deactivate();
			  turn2.myActivate = 0;
			  turn2.myDirection = 0;
			  robot.lock();
		  }
		  else if (goalNum == 5)
		  {
			gotoPoseAction.setGoal(ArPose(650, 2670));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		  else if (goalNum == 6)
		  {
			  printf("Gira 90 grados izquierda\n");
			  robot.unlock();
			  turn2.myActivate = 1;
			  turn2.myDirection = 1;
			  turn2.activate();
			  ArUtil::sleep(1000);
			  turn2.deactivate();
			  turn2.myActivate = 0;
			  turn2.myDirection = 0;
			  robot.lock();
		  }
		  else if (goalNum == 7)
		  {
			gotoPoseAction.setGoal(ArPose(650, 0));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		  else if (goalNum == 8)
		  {
			gotoPoseAction.setGoal(ArPose(1800,1199));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		  else if (goalNum == 9)
		  {
			gotoPoseAction.setGoal(ArPose(2600, 1199));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		  else if (goalNum == 10)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 850));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 2)
			  {
				gotoPoseAction.setGoal(ArPose(3500, 1199));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY()); 
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 1550));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 11)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 613));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 2)
			  {
				  printf("Gira 180 grados derecha\n");
				  robot.unlock();
				  turn2.myActivate = 1;
				  turn2.myDirection = 2;
				  turn2.activate();
				  ArUtil::sleep(2000);
				  turn2.deactivate();
				  turn2.myActivate = 0;
				  turn2.myDirection = 0;
				  robot.lock();
				  goalNum = 19;
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 1785));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 12)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(3300, 413));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(3300, 1985));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 13)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(3500, 413));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(3500, 1985));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 14)
		  {
			  robot.unlock();
			  //Valor para el while
			  bool fContinue = true;
				// <<<<<<------------- 1 Parte Secuencia: BAJA BRAZO ------------->>>>>> //
				lLastError = serial.Write("b");
				if (lLastError != ERROR_SUCCESS)
					return ::ShowError(serial.GetLastError(), _T("Unable to send data"));

				//-------------------------E S C U C H A   C O M ----------------------------//
				do
				{
					// Wait for an event
					lLastError = serial.WaitEvent();
					if (lLastError != ERROR_SUCCESS)
						return ::ShowError(serial.GetLastError(), _T("Unable to wait for a COM-port event."));

					// Save event
					const CSerial::EEvent eEvent = serial.GetEventType();

					// Handle break event
					if (eEvent & CSerial::EEventBreak)
					{
						printf("\n### BREAK received ###\n");
					}

					// Handle CTS event
					if (eEvent & CSerial::EEventCTS)
					{
						printf("\n### Clear to send %s ###\n", serial.GetCTS()?"on":"off");
					}

					// Handle DSR event
					if (eEvent & CSerial::EEventDSR)
					{
						printf("\n### Data set ready %s ###\n", serial.GetDSR()?"on":"off");
					}

					// Handle error event
					if (eEvent & CSerial::EEventError)
					{
						printf("\n### ERROR: ");
						switch (serial.GetError())
						{
						case CSerial::EErrorBreak:		printf("Break condition");			break;
						case CSerial::EErrorFrame:		printf("Framing error");			break;
						case CSerial::EErrorIOE:		printf("IO device error");			break;
						case CSerial::EErrorMode:		printf("Unsupported mode");			break;
						case CSerial::EErrorOverrun:	printf("Buffer overrun");			break;
						case CSerial::EErrorRxOver:		printf("Input buffer overflow");	break;
						case CSerial::EErrorParity:		printf("Input parity error");		break;
						case CSerial::EErrorTxFull:		printf("Output buffer full");		break;
						default:						printf("Unknown");					break;
						}
						printf(" ###\n");
					}

					// Handle ring event
					if (eEvent & CSerial::EEventRing)
					{
						printf("\n### RING ###\n");
					}

					// Handle RLSD/CD event
					if (eEvent & CSerial::EEventRLSD)
					{
						printf("\n### RLSD/CD %s ###\n", serial.GetRLSD()?"on":"off");
					}

					// Handle data receive event
					if (eEvent & CSerial::EEventRecv)
					{
						// Read data, until there is nothing left
						DWORD dwBytesRead = 0;
						char szBuffer[101];
						do
						{
							// Lee datos del Puerto COM
							lLastError = serial.Read(szBuffer,sizeof(szBuffer)-1,&dwBytesRead);
							if (lLastError != ERROR_SUCCESS)
								return ::ShowError(serial.GetLastError(), _T("Unable to read from COM-port."));

							if (dwBytesRead > 0)
							{
								//Preseteo color
								int color = 0;
								// Finaliza el dato, asi que sea una string valida
								szBuffer[dwBytesRead] = '\0';
								// Display the data
								printf("%s", szBuffer);

								// <<<<<<----------- 2 Parte Secuencia: CIERRA GRIPPER ----------->>>>>> //
								if (strchr(szBuffer,76))
								{
									lLastError = serial.Write("c");
									if (lLastError != ERROR_SUCCESS)
										return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
								}
								
								// <<<<<<------------- 3 Parte Secuencia: SUBE BRAZO ------------->>>>>> //
								if (strchr(szBuffer,117))
								{
									lLastError = serial.Write("s");
									if (lLastError != ERROR_SUCCESS)
										return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
								}

								// <<<<<<------------- 4 Parte Secuencia: COLOR ------------->>>>>> //
								if (strchr(szBuffer,72))
								{
									lLastError = serial.Write("C");
									if (lLastError != ERROR_SUCCESS)
										return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
								}

								// <<<<<<---------- 5.1 Parte Secuencia: COLOR ROJO---------->>>>>> //
								if (strchr(szBuffer,82))
								{
									color = 1;
									//salir del bucle
									fContinue = false;
								}

								// <<<<<<---------- 5.2 Parte Secuencia: COLOR AZUL ---------->>>>>> //
								if (strchr(szBuffer,66))
								{
									color = 2;
									//salir del bucle
									fContinue = false;
								}

								// <<<<<<---------- 5.3 Parte Secuencia: COLOR VERDE ---------->>>>>> //
								if (strchr(szBuffer,71))
								{
									color = 3;
									//salir del bucle
									fContinue = false;
								}
							}
						}
						while (dwBytesRead == sizeof(szBuffer)-1);
					}
				}
				while (fContinue);
				// Close the port again
				serial.Close();
				robot.lock();
		  }
		  else if (goalNum == 15)
		  {
			  printf("Gira 180 grados derecha\n");
			  robot.unlock();
			  turn2.myActivate = 1;
			  turn2.myDirection = 2;
			  turn2.activate();
			  ArUtil::sleep(2000);
			  turn2.deactivate();
			  turn2.myActivate = 0;
			  turn2.myDirection = 0;
			  robot.lock();
		  }
		  else if (goalNum == 16)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(3300, 413));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(3300, 1985));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 17)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 603));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 1795));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 18)
		  {
			  if (color == 1)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 860));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
			  if (color == 3)
			  {
				gotoPoseAction.setGoal(ArPose(2800, 1540));
				ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
				gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
			  }
		  }
		  else if (goalNum == 19)
		  {
			gotoPoseAction.setGoal(ArPose(2600, 1199));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		  else if (goalNum == 20)
		  {
			gotoPoseAction.setGoal(ArPose(1800, 1199));
			ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f", 
			gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
		  }
		}

		if(start.mSecSince() >= duration) {
		  ArLog::log(ArLog::Normal, "No puede llegar al punto, y la aplicacion saldra en %d", duration/1000);
		  gotoPoseAction.cancelGoal();
		  robot.unlock();
		  ArUtil::sleep(3000);
		  break;
		}
	    
		robot.unlock();
		ArUtil::sleep(10);
	  }

	  // Robot desconectado al terminal el sleep
	  Aria::shutdown();

	//------------ F I N   M A I N    D E L   P R O G R A M A   D E L    R O B O T-----------//
    
    return 0;
}
Exemple #6
0
int main (int argc, char** argv) {
	Aria::init();
	ArRobot robot;
	ArSonarDevice sonar;
	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())
		{
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}

	ArSonarDevice sonarDev;
	ArPose* poseList = readPostitions("positions.txt");
	robot.runAsync(true);
	robot.enableMotors();
	robot.moveTo(ArPose(0,0,0));
	robot.comInt(ArCommands::ENABLE, 1);
	robot.addRangeDevice(&sonarDev);
	ArActionGoto gotoPoseAction("goto", ArPose(0, 0, 0), 200);
	ArActionAvoidFront avoidFront("avoid front");
	ArActionStallRecover stallRecover("stall recover");
	robot.addAction(&gotoPoseAction, 50);
	robot.addAction(&avoidFront, 60);
	robot.moveTo(ArPose(0,0,0));
	int length = ARRAY_SIZE(poseList);
	cout<<"do dai"<<length;
	ArServerBase server;
	ArServerSimpleOpener simpleOpener(&parser);
	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);
	  }
	ArServerInfoRobot serverInfo(&server, &robot);
	GotoGoal gotoGoal(&robot, &sonar, &server, &serverInfo);
	gotoGoal.init(argc, argv);
	float angle = 0;
	VideoCapture cap;
	cap.open(0);
	Rect trackWindow;
	//var check find ball
	bool checkObject = false;
	int hsize = 16;

	namedWindow( "threshold", 0 );
	namedWindow( "trackbar", 0 );
	namedWindow( "Histogram", 0 );
	namedWindow( "main", 0 );
	createTrackbar( "Vmin", "trackbar", &vmin, 256, 0 );
	createTrackbar( "Vmax", "trackbar", &vmax, 256, 0 );
	createTrackbar( "Smin", "trackbar", &smin, 256, 0 );

	CascadeClassifier c;
	c.load("cascade.xml");
	Mat frame, hsv, hue, mask, hist, histimg = Mat::zeros(200, 320, CV_8UC3), backproj;
	float vel = 0;
	int i = 0;
	while(1)
	{
		cap >> frame;
		if( frame.empty() ){
			cout<<"error camera"<<endl;
			break;
		}
		frame.copyTo(image);
		cvtColor(image, hsv, COLOR_BGR2HSV);
		int _vmin = vmin, _vmax = vmax;
		inRange(hsv, Scalar(0, smin, MIN(_vmin,_vmax)),	Scalar(180, 256, MAX(_vmin, _vmax)), mask);
		gotoPoseAction.setGoal(poseList[i]);
		while (!gotoPoseAction.haveAchievedGoal()) 
		{
			ArLog::log(ArLog::Normal, "goal(%.2f, %0.2f) x = %.2f, y = %.2f", poseList[i].getX(), poseList[i].getY(), robot.getX(), robot.getY());
//			if (!checkObject)
			   checkObject = detect(frame, c);
			if (checkObject)
				cout <<"Phat hien doi tuong"<<endl;
			else
				cout <<"Khong phat hien doi tuong"<<endl;
			if (checkObject) {
				if(trackObject(hsv, mask)) {
					float d = distance();
					if (d < 250) {
						gotoGoal.move(-200);
					} else if ( d >= 250 && d <= 300) {
						gotoGoal.stop();
					}
					else {
						vel = d * 0.7;
						vel = (int) (vel/50) * 50;
						if(vel > 200) {
							vel = 200;
							gotoGoal.setVel(vel);
						}
						angle =  determindRotate();
						cout <<"khoang cach: "<<d<<"\tGoc quay: "<<angle<<"\t van toc = "<<vel<<endl;
						if (angle != 0) {
							gotoGoal.stop();
							gotoGoal.rotate(angle);
						}
					}
				}
			}
			imshow("main", image);
			imshow( "threshold", mask );
			imshow( "Histogram", histimg );
		}
		i++;
	}

	ArUtil::sleep(2000);
	Aria::shutdown();

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

  Aria::init();
  ArRobot robot;
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();

  ArLog::log(ArLog::Terse, "WARNING: this program does no sensing or avoiding of obstacles, the robot WILL collide with any objects in the way! Make sure the robot has approximately 3 meters of free space on all sides.");

  // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
  // and then loads parameter files for this robot.
  ArRobotConnector robotConnector(&parser, &robot);
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "simpleMotionCommands: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
        Aria::logOptions();
        Aria::exit(1);
        return 1;
    }
  }
  if (!Aria::parseArgs())
  {
    Aria::logOptions();
    Aria::exit(1);
    return 1;
  }
  
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Connected.");

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

  // Print out some data from the SIP.  

  // We must "lock" the ArRobot object
  // before calling its methods, and "unlock" when done, to prevent conflicts
  // with the background thread started by the call to robot.runAsync() above.
  // See the section on threading in the manual for more about this.
  // Make sure you unlock before any sleep() call or any other code that will
  // take some time; if the robot remains locked during that time, then
  // ArRobot's background thread will be blocked and unable to communicate with
  // the robot, call tasks, etc.
  
  robot.lock();
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
    robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage());
  robot.unlock();

  // Sleep for 3 seconds.
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Will start driving in 3 seconds...");
  ArUtil::sleep(3000);

  // Set forward velocity to 50 mm/s
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 250 mm/s for 5 sec...");
  robot.lock();
  robot.enableMotors();
  robot.setVel(250);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(1000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at 10 deg/s for 5 sec...");
  robot.lock();
  robot.setRotVel(10);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at -10 deg/s for 10 sec...");
  robot.lock();
  robot.setRotVel(-10);
  robot.unlock();
  ArUtil::sleep(10000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 150 mm/s for 5 sec...");
  robot.lock();
  robot.setRotVel(0);
  robot.setVel(150);
  robot.unlock();
  ArUtil::sleep(5000);

  ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(1000);


  // Other motion command functions include move(), setHeading(),
  // setDeltaHeading().  You can also adjust acceleration and deceleration
  // values used by the robot with setAccel(), setDecel(), setRotAccel(),
  // setRotDecel().  See the ArRobot class documentation for more.

  
  robot.lock();
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
    robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage());
  robot.unlock();

  
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Ending robot thread...");
  robot.stopRunning();

  // wait for the thread to stop
  robot.waitForRunExit();

  // exit
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
  Aria::exit(0);
  return 0;
}
int _tmain(int argc, char** argv)
{
    //-------------- 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);
    ActionTurns turn(400, 55);
    robot.addAction(&turn, 49);
    ActionTurns turn2(400, 55);
    robot.addAction(&turn2, 49);
    turn.deactivate();
    turn2.deactivate();

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

    bool first = true;
    int goalNum = 0;
    int color = 3;
    ArTime start;
    start.setToNow();
    while (Aria::getRunning())
    {
        robot.lock();

        // inicia el primer punto
        if (first || gotoPoseAction.haveAchievedGoal())
        {
            first = false;

            goalNum++; //cambia de 0 a 1 el contador
            printf("El contador esta en: --> %d <---\n",goalNum);
            if (goalNum > 20)
                goalNum = 1;

            //comienza la secuencia de puntos
            if (goalNum == 1)
            {
                gotoPoseAction.setGoal(ArPose(1150, 0));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                // 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();
            }
            else if (goalNum == 2)
            {
                printf("Gira 90 grados izquierda\n");
                robot.unlock();
                turn.myActivate = 1;
                turn.myDirection = 1;
                turn.activate();
                ArUtil::sleep(1000);
                turn.deactivate();
                turn.myActivate = 0;
                turn.myDirection = 0;
                robot.lock();
            }
            else if (goalNum == 3)
            {
                gotoPoseAction.setGoal(ArPose(1150, 2670));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                // 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();
            }
            else if (goalNum == 4)
            {
                printf("Gira 90 grados izquierda\n");
                robot.unlock();
                turn2.myActivate = 1;
                turn2.myDirection = 1;
                turn2.activate();
                ArUtil::sleep(1000);
                turn2.deactivate();
                turn2.myActivate = 0;
                turn2.myDirection = 0;
                robot.lock();
            }
            else if (goalNum == 5)
            {
                gotoPoseAction.setGoal(ArPose(650, 2670));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
            else if (goalNum == 6)
            {
                printf("Gira 90 grados izquierda\n");
                robot.unlock();
                turn2.myActivate = 1;
                turn2.myDirection = 1;
                turn2.activate();
                ArUtil::sleep(1000);
                turn2.deactivate();
                turn2.myActivate = 0;
                turn2.myDirection = 0;
                robot.lock();
            }
            else if (goalNum == 7)
            {
                gotoPoseAction.setGoal(ArPose(650, 0));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
            else if (goalNum == 8)
            {
                gotoPoseAction.setGoal(ArPose(1800,1199));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
            else if (goalNum == 9)
            {
                gotoPoseAction.setGoal(ArPose(2600, 1199));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
            else if (goalNum == 10)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 850));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 2)
                {
                    gotoPoseAction.setGoal(ArPose(3500, 1199));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 1550));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 11)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 613));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 2)
                {
                    printf("Gira 180 grados derecha\n");
                    robot.unlock();
                    turn2.myActivate = 1;
                    turn2.myDirection = 2;
                    turn2.activate();
                    ArUtil::sleep(2000);
                    turn2.deactivate();
                    turn2.myActivate = 0;
                    turn2.myDirection = 0;
                    robot.lock();
                    goalNum = 19;
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 1785));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 12)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(3300, 413));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(3300, 1985));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 13)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(3500, 413));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(3500, 1985));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 14)
            {
                //secuencia de drop de la figura
            }
            else if (goalNum == 15)
            {
                printf("Gira 180 grados derecha\n");
                robot.unlock();
                turn2.myActivate = 1;
                turn2.myDirection = 2;
                turn2.activate();
                ArUtil::sleep(2000);
                turn2.deactivate();
                turn2.myActivate = 0;
                turn2.myDirection = 0;
                robot.lock();
            }
            else if (goalNum == 16)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(3300, 413));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(3300, 1985));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 17)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 603));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 1795));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 18)
            {
                if (color == 1)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 860));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
                if (color == 3)
                {
                    gotoPoseAction.setGoal(ArPose(2800, 1540));
                    ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                               gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
                }
            }
            else if (goalNum == 19)
            {
                gotoPoseAction.setGoal(ArPose(2600, 1199));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
            else if (goalNum == 20)
            {
                gotoPoseAction.setGoal(ArPose(1800, 1199));
                ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
                           gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
            }
        }

        if(start.mSecSince() >= duration) {
            ArLog::log(ArLog::Normal, "No puede llegar al punto, y la aplicacion saldra en %d", duration/1000);
            gotoPoseAction.cancelGoal();
            robot.unlock();
            ArUtil::sleep(3000);
            break;
        }

        robot.unlock();
        ArUtil::sleep(10);
    }

    // Robot desconectado al terminal el sleep
    Aria::shutdown();

//----------------------------------------------------------------------------------//

    return 0;
}
Exemple #9
0
void Joydrive::drive(void)
{
  int trans, rot;
  ArPose pose;
  ArPose rpose;
  ArTransform transform;
  ArRangeDevice *dev;
  ArSensorReading *son;

  if (!myRobot->isConnected())
  {
    printf("Lost connection to the robot, exiting\n");
    exit(0);
  }
  printf("\rx %6.1f  y %6.1f  th  %6.1f", 
	 myRobot->getX(), myRobot->getY(), myRobot->getTh());
  fflush(stdout);
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
  {
    if (ArMath::fabs(myRobot->getVel()) < 10.0)
      myRobot->comInt(ArCommands::ENABLE, 1);
    myJoyHandler.getAdjusted(&rot, &trans);
    myRobot->setVel(trans);
    myRobot->setRotVel(-rot);
  }
  else
  {
    myRobot->setVel(0);
    myRobot->setRotVel(0);
  }
  if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) &&
      time(NULL) - myLastPress > 1)
  {
    myLastPress = time(NULL);
    printf("\n");
    switch (myTest)
    {
    case 1:
      printf("Moving back to the origin.\n");
      pose.setPose(0, 0, 0);
      myRobot->moveTo(pose);
      break;
    case 2:
      printf("Moving over a meter.\n");
      pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
      myRobot->moveTo(pose);
      break;
    case 3:
      printf("Doing a transform test....\n");
      printf("\nOrigin should be transformed to the robots coords.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(0, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
    
      printf("\nRobot coords should be transformed to the origin.\n");
      transform = myRobot->getToLocalTransform();
      pose = myRobot->getPose();
      pose = transform.doTransform(pose);
      rpose.setPose(0, 0, 0);
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();
      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 4:
      printf("Doing a tranform test...\n");
      printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(-1000, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
	printf("Probable Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 5:
      printf("Doing a transform test on range devices..\n");
      printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
      dev = myRobot->findRangeDevice("sonar");
      if (dev == NULL)
      {
	printf("No sonar on the robot, can't do the test.\n");
	break;
      }
      printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
      printf("Sonar 0 reading is at ");
      son = myRobot->getSonarReading(0);
      if (son != NULL)
      {
	pose = son->getPose();
	pose.log();
      }
      pose = myRobot->getPose();
      pose.setX(pose.getX() + 4000);
      pose.setY(pose.getY() + 4000);
      myRobot->moveTo(pose);
      printf("Moved robot.\n");
      printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
      printf("Sonar 0 reading is at ");
      son = myRobot->getSonarReading(0);
      if (son != NULL)
      {
	pose = son->getPose();
	pose.log();
      }

      break;
    case 6:
      printf("Robot position now is:\n");
      pose = myRobot->getPose();
      pose.log();
      printf("Disconnecting from the robot, then reconnecting.\n");
      myRobot->disconnect();
      myRobot->blockingConnect();      
      printf("Robot position now is:\n");
      pose = myRobot->getPose();
      pose.log();
      break;
    default:
      printf("No test for second button.\n");
      break;
    } 
  }
}