Diseñando la Aplicación Cliente

Buenas,

puesto que en la parte servidora (el corazón de la aplicación) ya estan implementados los 6 modos de funcionamiento, ahora toca diseñar una aplicación cliente que se conecte y lance el modo seleccionado o en caso del control manual pueda enviar los comandos.

Para esto, estoy planteando una aplicación MFC basada en un cuadro de dialogo que contiene el control. Un boceto de la interfaz de usuario sería algo similar a esto:


Aunque a primera vista parecía sencillo, aparte de tener que casi aprender MFC en unos dos dias, la cosa se pudo bastante fea al toparme con el término de "functores". Los functores son plantillas de punteros a funciones (que ya suena a chungo) que se han inventado la gente de Aria y que básicamente sirven para llamar a la función que apuntan en determinados momentos. En este caso servirá para manejar las respuestas del servidor a las peticiones del cliente.

Otro problema fue, la actualización de los componentes del dialogo cuando escribe el método de callback (la llamada del functor), ya que este es invocado por un hilo (el cliente, que se ejecuta de manera asíncrona, y este no puede usar la función UpdateData(bool), que actualiza los componentes con sus variables asignadas, y en este caso se usaría para, por ejemplo, actualizar la información del log con la información que recive de los mensajes. Para esto en vez de usar UpdateData hay que usar "SetWindowText" que escribe directamente sobre el componente y no da problemas con el Threading.

Por último, comentar que he diseñado un icono para aplicación que representa al robot y una marca del Artoolkit

Conversaciones Cliente-Servidor

Buenas,

puesto que se pretente hacer una arquitectura cliente-servidor para iniciar los modos de funcionamiento, obtener los datos y la imagen que el robot captura o para poder controlarlo en el modo manual, nos hace falta definir un protocolo de mensajes para que puedan conversar entre el cliente y el servidor. Para este proposito he definido el siguiente formato de tramas de mensajes que intercambian:

Client-Server Communication

Init robot (TCP):

Client request:

SMS_INIT

Server response confirmation:

SMS_INIT

SMS_ACK

"Server state: INIT"

Server response error:

SMS_INIT

SMS_CRITICAL_ERROR

"server can't init due to call has failed"

Exe Mode N (TCP):

Client request:

SMS_EXE_MODE_N

Server response confirmation:

SMS_EXE_MODE_N

SMS_ACK

“(Server State): Running mode N”

MapaData(*)

(*) Only in modes 1, 2 and 3

Server response error:

SMS_EXE_MODE_N

SMS_CRITICAL_ERROR

1)"server can't run strategy due to the state wasn't ST_INIT" 2) "server can't run strategy due to init function failed"

Robot and Camera commands (only on mode 6) (UDP):

Client request:

SMS_COM_ADVANCE | SMS_COM_GO_BACK | SMS_COM_TURN_RIGHT | SMS_COM_TURN_LEFT | SMS_COM_TILT_UP | SMS_COM_TILT_DOWN | SMS_COM_PAN_RIGHT | SMS_COM_PAN_LEFT | SMS_COM_CAM_RST | SMS_COM_STOP

Cancel active mode (TCP):

Client request:

SMS_CANCEL

Server response confirmation:

SMS_CANCEL

SMS_ACK

"Stopped mode N"

Server response error:

SMS_CANCEL

SMS_WARNING

"Couldn't stop mode due to it wasn's running"

Get data without image (UDP):

Client request:

SMS_RETURN_TEXT

Server response:

SMS_RETURN_TEXT	GoalReached (*)	PosRobotX	PosRobotY	RobotTh
idMark	posMarkX	posMarkY	MarkTh	UseMarkInfo (**)
“date and time”

(*) Some modes finished when reach one goal: 0 or 1

(**) 0 or 1 if use or not the position estimation.

Get data with image (UDP):

Client request:

SMS_RETURN_IMAGE

Server response:

SMS_RETURN_IMAGE	GoalReached (*)	PosRobotX	PosRobotY	RobotTh
idMark	posMarkX	posMarkY	MarkTh	UseMarkInfo (**)
“date and time”	ImageData

(*) Some modes finished when reach one goal: 0 or 1

(**) 0 or 1 if use or not the position estimation.

Saludos!!

Modos de funcionamiento implementados

Una vez con los datos que nos proporcionan las marcas se han implementado los siguientes modos de funcionamiento:

1) Seguimiento de una ruta planificada sobre un mapa sin asistencia de marcas
2) Seguimiento de una ruta planificada sobre un mapa recogiendo información aportada por las marcas del entorno.
3) Seguimiento de una ruta planificada sobre un mapa usando la información que considera de utilidad de la información aportada por las marcas del entorno.
4) Seguimiento de una ruta que es establecida solo por marcas visuales, sin el uso de mapas.
5) Seguimiento de una marca en movimiento.

Por otro lado también tengo pensado (si me diera tiempo) hacer un modo manual, donde un usuario controla remotamente el robot y la cámara.

Ahora la aplicacion esta corriendo directamente sobre el robot, pero se intentara implementar una aplicacion cliente que inicie el servicio haciendo uso de la librería ArNetworking, escoja el modo que quiere ejecutar y pueda ver la imagen que el robot esta capturando. Este ultimo punto puede crear un cuello de botella en el procesamiento al tener que comprimir la imagen y enviarla en tiempo real.

Hasta pronto!

Obteniendo la posicion mediante marcas

Buenas,

tras probar el caso anterior, en la negación sin marcas se observa que la odometria comente ciertos errores de medición.

Ahora, se plantea un caso de estudio, donde el robot va navegando normalmente y cuando se encuentra una marca calcula su posición en el mapa con respecto la posición global de la marca en el mapa. Aparte, cada vez que detecta una marca muestra la información por pantalla y añade una entrada en un log con la siguiente información:

-Fecha y hora en que se ha encontrado la marca
-Posición del robot en el mapa que se estima mediante el odómetro y el giroscopio.
-Posición del robot en el mapa que estima mediante la información que le aporta la marca.

Con esto, aparte de poder depurar de forma visual en tiempo de ejecución, se proporciona un log que se puede consultar a posteriori para hacer comparaciones. En la siguiente figura se puede observar lo comentado anteriormente.



Para la prueba se ha situado la marca a un metro del robot y el error aproximado según se observa en las entradas del log es de unos 30-50 milímetros como máximo Lo cual parece un resultado positivo para trabajar en futuros modos de funcionamiento que usen esta información para tomar determinadas decisiones.

Como comentario, cabe anotar que se ha sustituido el uso de jpegLib por OpenCV [1], ya que tiene una mayor funcionalidad y en nuestro caso nos permitirá operaciones como:
-Añadir información al frame(como texto o formas básicas)
-Realizar diferentes transformaciones (lo que se ha usado para realizarle un flip vertical a la imagen, pues la capturadora la proporciona invertida)
-Guardar un frame en diferentes formas (aparte del jpeg).
-Mostrar la imagen en una ventana de forma sencilla.

[1] http://sourceforge.net/projects/opencvlibrary/

Moviendo a Don Pioneer 3AT

Buenas,

una vez asentadas las bases del funcionamiento de Artoolkit, Aria, etc... me dispongo a implementar un caso de estudio para comprobar la funcionalidad que podría tener el uso de marcas para conseguir un posicionamiento del robot más robusto y fiable.

Para este objetivo he creado un mapa de la segunda planta del I3A con la herramienta Mapper3Basic [1].

Primeramente se intentó hacer una implementación de un algoritmo de navegación híbrida que se componía de un algoritmo de planificación global basado en rejillas (que divide el mapa en casillas y calcula la mínima distancia de un origen a un destino) para calcular por qué puertas debía pasar y un algoritmo de navegación local (en concreto se usó el algoritmo Vector Field Histograms [2]) para esquivar obstáculos inesperados que se pudieran presentar durante la navegación.

Tras la implementación del algoritmo se probó que la navegación local es impracticable debido a las fluctuaciones de los sonares (quizá con la herramienta del láser se obtendría mayor precisión y sería aplicable).

A la vista de esto, como el desarrollo de un algoritmo de navegación no es parte del objetivo del proyecto (aunque si me hubiera gustado que funcionara, aunque fuera por probar alguna cosa más) se ha opatado por definir una ruta preestablecida en el mapa (como se puede ver en la siguiente figura) para empezar observar el error de odometría cometido y probar diferentes estategias en las cuales las marcas podrían aportar cierta semántica al entorno para mejorar el posicionamiento en la navegación.




[1] http://robots.mobilerobots.com/Mapper3Basic/
[2] http://www-personal.umich.edu/~johannb/vff&vfh.htm

Jugando con las coordenadas de ARToolkitPlus

Una vez puesto en marcha Artoolkit Plus, presenta dos ejemplos muy básicos.

En el caso de detectar una sola marca te devuelve una matriz de transformación que representa la posición de la marca con respecto a la cámara. La salida del programa es la siguiente:

ARToolKitPlus: CamSize 320 , 240
ARToolKitPlus: Dist.Factor 159.00 139.00 -84.90 0.98


Found marker 100  (confidence 100%)

Pose-Matrix:
0.28  0.58  -0.77  0.00
  0.96  -0.20  0.20  0.00
  -0.04  -0.79  -0.61  0.00
6.29  -3.43  265.68  1.00

Esta matriz representa la orientación mediante una matriz 3x3 (macada en negrita) y la dustancia x, y, z (marcadas en rojo).

La duda que se plantea es cómo pasar de una matriz 3x3 a tres ángulos de rotación (uno por eje).

Teniendo en cuenta que la matriz de transformación esta basada en la matriz de OpenGl, la matriz de rotación se obtiene de la multiplicación de la multiplicación de las tres las matrices de rotación:

Realizando las siguientes simplificaciones: cx=Cos(x), cy=Cos(y), cz=Cos(z), stx=Sin(x), sty=Sin(y), stz=Sin(z), y multiplicando las tres rotaciones nos que daría la siguiente matriz:

[R] =

rotate about z (last rotation) cz -sz 0 sz cz 0 0 0 1

* (

rotate about y (second rotation) cy 0 sy 0 1 0 -sy 0 cy

*

rotate about x (first rotation) 1 0 0 0 cx -sx 0 sx cx

)

[R] =

cy * cz	sx*sy*cz-cx*sz	cx*sy*cz+sx*sz
cy * sz	sx*sy*sz +cx*cz	cx*sy*sz-sx*cz
-sy	sx*cy	cx*cy

De donde es fácil ver que:

Ang x = atan(r[2][1]/r[2][2])
Ang y = asin(-r[2][0])
Ang z = atan(r[0][0]/r[0][1])

De estas operaciones se observa que se puede producir "Gimbal Lock" si se divide por algún seno o coseno que tenga valor 0. Como para este caso solo hace falta saber el giro del eje Y, y se obtiene de asin(-sy) solo puede valer 0 en -90 o +90 grados, lo que no se da nunca porque con esa inclinación la cámara no ve la marca. En cualquier caso, si se diera esta problema puede consultarse el siguiente enlace para resolverlo [1].

Por otro lado también comentar que si queremos la posición de la cámara respecto a la marca, se llamará a la función arUtilMatInv de artoolkit ya que artoolkit plus no incluye esta función.

Por ahora es todo.

[1] http:/www.j3d.org/matrix_faq/matrfaq_latest.html#Q37