Código de la aplicación

/*

DETECCIÓN Y SEGUIMIENTO DE VISITANTES CON ROBOT MÓVIL Alfredo Morales Pinzón

Asesor: Fernando De la Rosa Universidad de los Andes

Archivo:servidorSeguidor.cpp

En este archivo se encuentra en código fuente de la aplicación desarrollada para la detección y el seguimiento de visitantes con robot móvil.

Este código ha sido generado a partir de códigos base de MobileRobots. En especifico se usaron los ejemplos: actsColorFollowingExample y serverDemo (ArNetworking).

*/

//Librerias

#include "Aria.h"

#include "ArNetworking.h"

//Clase "Seguir" que hace la rutina de detección y seguimiento del visitante.

//Es importante anotar que el método fire de la clase es el que se invocará

//por cada ciclo de ejecución del robot. class Seguir : public ArAction

{

public:

// The state of the Seguir action enum State {

NO_TARGET, // There is no target in view TARGET, // This is a target in view };

// Constructor

Seguir(ArACTS_1_2 *acts, ArVCC4 *camera, ArKeyHandler *keyHandler);

// Destructor ~Seguir(void);

// Activa la rutina de detección y seguimiento void activar();

//Desactiva la rutina de detección y seguimiento void desactivar();

// Cambia parámetros de movimiento para avanzar void avanzar();

// Cambia parámetros de movimiento para frenar (Disminuir velocidad)

41

void frenar();

// Cambia parámetros de movimiento para parar void parar();

// Cambia parámetros de movimiento para girar a la derecha void girarDerecha();

// Cambia parámetros de movimiento para girar a la izquierda void girarIzquierda();

// The action

ArActionDesired *fire(ArActionDesired currentDesired);

// Set the ACTS channel that we want to get blob info from bool setChannel(int channel);

// Return the current state of this action State getState(void) { return myState; }

// Height and width of pixels from frame-grabber enum {

WIDTH = 160, HEIGHT = 120 };

protected:

// Action que se desea ejecute el robot ArActionDesired myDesired;

// Se encargada de hacer la conexión con ACTS ArACTS_1_2 *myActs;

// Se encarga de hacer la conexión con la cámara para su control ArVCC4 *myCamera;

// Ultimo tiempo en que se vio el objeto de detección ArTime myLastSeen;

// Estado de la clase State myState;

//Canal de obtención de datos de ACTS int myChannel;

// Máximo tiempo de espera para obtener una detección int myMaxTime;

// Handler de teclado

ArKeyHandler *myKeyHandler;

// Funtores de los métodos de movimiento. Se usan para estableces // funciones de callback. ArFunctorC<Seguir> activarCB; ArFunctorC<Seguir> desactivarCB; ArFunctorC<Seguir> avanzarCB; ArFunctorC<Seguir> frenarCB; ArFunctorC<Seguir> pararCB; ArFunctorC<Seguir> girarDerechaCB; ArFunctorC<Seguir> girarIzquierdaCB;

// Estado de la rutina de seguimiento (Activada/Desactivada) bool seguirActivado;

// Estado del movimiento dado por el usuario mediante el teclado bool mover;

42

int deltaAngulo;

// Cantidad de movimiento incremental dada por el usuario al mover el robot

int deltaVelocidad;

// Numero de incrementos en velocidad (Positivos o Negativos) int adelante;

// Numero de incrementos en rotación (Positivos o Negativos) int izquierda;

};

// Constructor: Initialize the Seguir action

Seguir::Seguir(ArACTS_1_2 *acts, ArVCC4 *camera, ArKeyHandler *keyHandler) :

myKeyHandler(keyHandler), ArAction("Seguir", "Seguir the largest blob."),activarCB(this, &Seguir::activar)

,desactivarCB(this, &Seguir::desactivar), avanzarCB(this, &Seguir::avanzar), pararCB(this, &Seguir::parar)

,girarDerechaCB(this, &Seguir::girarDerecha),girarIzquierdaCB(this, &Seguir::girarIzquierda), frenarCB(this, &Seguir::frenar)

{ //Inicialización de atributos myActs = acts; myCamera = camera; myChannel = 0; myState = NO_TARGET; setChannel(1); myLastSeen.setToNow(); myMaxTime = 1000; seguirActivado = false; mover = false; adelante = 0; izquierda = 0; deltaVelocidad = 15; deltaAngulo = 12;

//Se añaden las funciones de CallBack al handler del teclado myKeyHandler->addKeyHandler('a', &activarCB); myKeyHandler->addKeyHandler('d', &desactivarCB); myKeyHandler->addKeyHandler(ArKeyHandler::UP, &avanzarCB); myKeyHandler->addKeyHandler(ArKeyHandler::DOWN, &frenarCB); myKeyHandler->addKeyHandler(ArKeyHandler::SPACE, &pararCB); myKeyHandler->addKeyHandler(ArKeyHandler::RIGHT, &girarDerechaCB); myKeyHandler->addKeyHandler(ArKeyHandler::LEFT, &girarIzquierdaCB); } // Destructor Seguir::~Seguir(void) {

//Cerramos la conexión con ACTS if(myActs->isConnected()) { myActs->requestQuit(); myActs->closePort(); printf("Se desconecto\n"); }

43

}

// Método llamado al oprimir la tecla "a", para activar la rutina // de seguimiento void Seguir::activar(void) { printf("activar\n"); seguirActivado = true; }

// Método llamado al oprimir la tecla "d", para desactivar la rutina // de seguimiento void Seguir::desactivar(void) { printf("desactivar\n"); seguirActivado = false; }

// Método llamado al oprimir la tecla "UP". Aumenta el parámetro de // velocidad del robot

void Seguir::avanzar(void) { if(!seguirActivado) { printf("Hacia arriba\n"); adelante += deltaVelocidad; mover = true; } }

// Método llamado al oprimir la tecla "DOWN". Disminuye el parámetro de // velocidad del robot

void Seguir::frenar(void) { if(!seguirActivado) { printf("Frenando\n"); adelante -= deltaVelocidad; mover = true; } }

// Método llamado al oprimir la tecla "SPACE". Pone en ceros los parámetros de

// movimiento del robot void Seguir::parar(void) { if(!seguirActivado) { printf("Parar\n"); mover = false; adelante = 0; izquierda = 0; } }

44

// Método llamado al oprimir la tecla "RIGHT". Disminuye el parámetro de

// Angulo de dirección del robot void Seguir::girarDerecha(void) { if(!seguirActivado) { printf("Girar Derecha\n"); mover = true; izquierda -= 1 ; } }

// Método llamado al oprimir la tecla "LEFT". Aumenta el parámetro de // Angulo de dirección del robot

void Seguir::girarIzquierda(void) { if(!seguirActivado) { printf("Girar Izquierda\n"); mover = true; izquierda += 1 ; } }

// Acción de seguir que se ejecuta en la rutina del robot ArActionDesired *Seguir::fire(ArActionDesired currentDesired) { // Reset myDesired.reset(); if(!seguirActivado) { if(mover) { myDesired.setVel(adelante); myDesired.setDeltaHeading(izquierda*deltaAngulo); izquierda = 0; } else { myDesired.setVel(0); myDesired.setDeltaHeading(0); } return &myDesired; }

// Se anula el movimiento del robot por parte del usuario adelante = 0;

izquierda = 0;

ArACTSBlob blob;

ArACTSBlob largestBlob;

45

int numberOfBlobs; int blobArea = 10;

double xRel, yRel;

numberOfBlobs = myActs->getNumBlobs(myChannel);

// If there are blobs to be seen, set the time to now if(numberOfBlobs != 0)

{

for(int i = 0; i < numberOfBlobs; i++) { myActs->getBlob(myChannel, i + 1, &blob); if(blob.getArea() > blobArea) { flag = true; blobArea = blob.getArea(); largestBlob = blob; } } myLastSeen.setToNow(); }

// If we have not seen a blob in a while... if (myLastSeen.mSecSince() > myMaxTime) {

if(myState != NO_TARGET) ArLog::log(ArLog::Normal, "Target Lost");

myState = NO_TARGET; }

else {

// If we see a blob and haven't seen one before..

if(myState != TARGET) ArLog::log(ArLog::Normal, "Target Aquired");

myState = TARGET; }

if(TARGET && flag == true) {

// Determine where the largest blob's center of gravity // is relative to the center of the camera

xRel = (double)(largestBlob.getXCG() - WIDTH/2.0) / (double)WIDTH;

yRel = (double)(largestBlob.getYCG() - HEIGHT/2.0) / (double)HEIGHT;

// Tilt the camera toward the blob if(!(ArMath::fabs(yRel) < .20)) { if (-yRel > 0) myCamera->tiltRel(1); else myCamera->tiltRel(-1); }

46

// Set the heading and velocity for the robot if (ArMath::fabs(xRel) < .10) { myDesired.setDeltaHeading(0); } else { if (ArMath::fabs(-xRel * 10) <= 10) myDesired.setDeltaHeading(-xRel * 10); else if (-xRel > 0) myDesired.setDeltaHeading(10); else myDesired.setDeltaHeading(-10); } myDesired.setVel(300); return &myDesired; } else { myDesired.setVel(0); myDesired.setDeltaHeading(0); return &myDesired; } }

// Set the channel that the blob info will be obtained from bool Seguir::setChannel(int channel)

{

if (channel >= 1 && channel <= ArACTS_1_2::NUM_CHANNELS) { myChannel = channel; return true; } else return false; }

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

// Inicializamos ARIA Aria::init();

//ArLog::init(ArLog::StdErr, ArLog::Verbose); ArRobot robot;

printf("Antes de crear la camara"); // The camera (Cannon VC-C4)

ArVCC4 vcc4 (&robot);

// ACTS, for tracking blobs of color ArACTS_1_2 acts;

// our base server object ArServerBase server;

// Parser de verificación de parametros ArArgumentParser parser(&argc, argv);

47

ArSimpleConnector simpleConnector(&parser); ArServerSimpleOpener simpleOpener(&parser);

// set up a gyro, if installed ArAnalogGyro gyro(&robot);

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

ArClientSwitchManager clientSwitchManager(&server, &parser);

if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) {

Aria::logOptions(); Aria::exit(1); }

// Se añade un key handler para el manejo de acciones por medio del teclado

ArKeyHandler keyHandler;

Aria::setKeyHandler(&keyHandler);

// Acciones limiter y limiter far para evitar que el robot choque ArActionLimiterForwards limiter("speed limiter near", 800, 1000, 600);

ArActionLimiterForwards limiterFar("speed limiter far", 800, 1200, 600);

ArActionLimiterBackwards backwardsLimiter; ArActionConstantVelocity stop("stop", 0);

ArActionConstantVelocity backup("backup", -200);

// Creacion de la clase que sigue al visitante Seguir seguir(&acts, &vcc4, &keyHandler);

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

48 ArIRs irs; robot.addRangeDevice(&irs); ArBumpers bumpers; robot.addRangeDevice(&bumpers); ArSick sick(361, 180); robot.addRangeDevice(&sick);

// Se añaden servicios al servidor

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

drawings.addRobotsRangeDevices(&robot);

ArLaserReflectorDevice reflector(&sick, &robot); drawings.addRangeDevice(&reflector);

// Modos de control de movimiento del robot ArServerModeStop modeStop(&server, &robot); ArServerModeDrive modeDrive(&server, &robot);

ArServerModeRatioDrive modeRatioDrive(&server, &robot); ArServerModeWander modeWander(&server, &robot);

modeDrive.addAsDefaultMode(); modeDrive.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

modeDrive.addControlCommands(&commands); // configure the drive modes (e.g. enable/disable safe drive)

// Reenviar el video al cliente que lo requiera

ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070);

// Control de cámara pan/tilt/zoom ArPTZ *camera = NULL;

ArServerHandlerCamera *handlerCamera = NULL; ArCameraCollection *cameraCollection = NULL;

49

if (videoForwarder.isForwardingVideo()) {

bool invertedCamera = false;

camera = new ArVCC4(&robot, invertedCamera, ArVCC4::COMM_UNKNOWN, true, true);

camera->init();

cameraCollection = new ArCameraCollection();

cameraCollection->addCamera("Cam1", "VCC4", "Camera", "VCC4");

handlerCamera = new ArServerHandlerCamera("Cam1", &server,

&robot, camera,

cameraCollection); }

// Conección con el robot

if (!simpleConnector.connectRobot(&robot)) {

printf("Could not connect to robot... exiting\n"); Aria::shutdown();

return 1; exit(1); }

// Abrir la conección con ACTS acts.openPort(&robot);

// Inicializa la cámara vcc4.init();

// Prevenir que el robot vaya muy rápido robot.setAbsoluteMaxTransVel(400);

// Habilitar la recepción de comandos robot.comInt(ArCommands::ENABLE, 1);

// Apagar los sonidos de amigobot robot.comInt(ArCommands::SOUNDTOG, 0);

// Se añadern las acciones con su respectivo nivel de importancia robot.addAction(&limiter, 100); robot.addAction(&limiterFar, 99); robot.addAction(&backwardsLimiter, 98); robot.addAction(&seguir, 77); robot.addAction(&backup, 50); robot.addAction(&stop, 30);

//Habilitar los motores robot.enableMotors();

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

50

server.runAsync();

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

// Se añade un key handler que maneje la salida oprimir escape //if ((keyHandler = Aria::getKeyHandler()) == NULL)

if (Aria::getKeyHandler() == NULL) {

keyHandler = new ArKeyHandler; Aria::setKeyHandler(&keyHandler); robot.lock();

robot.attachKeyHandler(&keyHandler); robot.unlock();

printf("To exit, press escape.\n"); }

robot.lock();

robot.attachKeyHandler(&keyHandler); robot.unlock();

printf("To exit, press escape.\n"); clientSwitchManager.runAsync();

//Corremos el robot asincronamente robot.runAsync(true);

//Esperamos escape para salir robot.waitForRunExit();

In document DETECCIÓN Y SEGUIMIENTO DE VISITANTE CON ROBOT MÓVIL (página 46-56)