P2: Rescue People

En esta práctica se nos pide detectar las caras de las personas desaparecidas en el mar y guardar sus localizaciones para que posteriormente se les rescate.

Para esta labor se nos proporcionan las coordenadas GPS de la plataforma desde la que parte el dron y de la zona aproximada donde se encuentran los supervivientes:

Safety boat  --> 40º16’48.2” N, 3º49’03.5” W

Survivors --> 40º16’47.23” N, 3º49’01.78” W

Retos

1. Despegar el dron

2. Ir a la zona donde se encuentran los supervivientes

3. Búsqueda de los supervivientes (detectando caras y guardando la coordenada donde se encuentran, evitando guardar a la misma persona varias veces)

4. Ir a la plataforma

5. Aterrizar
   

    
   

Primera etapa: Despegar  

Utilizando la función: HAL.takeoff(altura) conseguimos que nuestro dron despegue. 

Como todas las funciones de Unibotics son no bloqueantes es necesario hacer un control basado en estados. Por ello, esta parte estará en el estado despegando. 

Como condición de transición iré consultando la altura actual y comparando con la deseada, en el momento que sean prácticamente iguales pasaré al estado ir a la zona de los supervivientes.

Segunda etapa: Ir a la zona de los supervivientes

Unibotics nos proporciona diferentes controladores: en posición, velocidad y mixto. Para esta etapa he elegido el de posición, pero para ello hay que decirle las coordenadas en el mundo de Gazebo y nos proporcionan coordenadas GPS.

El primer paso será pasar de coordenadas GPS a UTM, para ello he usado el convertidor online del siguiente link. He obtenido los siguientes resultados:

Coordenadas UTM de la plataforma

Coordenadas UTM de la zona de los supervivientes

 

 Ya tenemos las coordenadas UTM, sabiendo que el (0, 0) de Gazebo está en la posición de la plataforma, restamos la coordenada UTM de los supervivientes menos la de la plataformay ya tendríamos las coordenadas en Gazebo de los supervivientes.

Esto se lo pasamos a HAL.set_cmd_pos(), aparte calculamos el ángulo para que el frente del robot mire al punto destino (arcotangente (survivors_y / survivors_x).

Como condición de transición comprobaré la posición actual y la deseada, cuando coincidan en un margen de decimales considero que ha llegado a la posición y paso al siguiente estado.

Tercera etapa: Buscar

Esta será la etapa más complicada porque se tiene que encargar de detectar caras, navegar, pasar de pixel a coordenada de Gazebo, ver si la coordenada de la persona detectada está lo suficientemente lejos de las ya detectadas como para considerarla una nueva persona y además saber cuándo dejar de buscar.

• Navegación

En esta parte lo que deseamos es cubrir el mayor área posible para detectar el mayor número de personas. Para esta parte decidí seguir la trayectoria de una espiral cuadrada. 

El primer paso fue generar una lista de puntos que el robot tendría que seguir. Esos puntos serán las esquinas de los cuadrados. Para que sea una espiral habrá que tener en cuenta que se debe partir de un tamaño de lado inicial, con este hacer dos lados e incrementar. 

Una vez que tenemos la lista de puntos toca seguirlos, para ello he elegido un control mixto para ir hacia delante y en posición para girar (ya que además de dejarlo en el ángulo exacto ayuda a corregir posibles errores anteriores). Para ir para delante he pasado las coordenadas a relativas y le he aplicado un control proporcional. Una mejora que añadí es que cuando estuviera a más de 10 metros del objetivo fuera a una velocidad constante de 1.5 m/s, ya que cuando estaba muy lejos alcanzaba velocidades demasiado altas como para detectar a las personas.

• Detección de caras 

Para detectar caras necesitaremos un fichero que será el clasificador pre-entrenado de caras, en Unibotics lo podremos encontrar en la siguiente ruta:

 

'RoboticsAcademy/exercises/static/exercises/rescue_people/haarcascade_frontalface_default.xml'

Como primera aproximación seguiremos el tutorial del siguiente link para detectar caras en una imagen. Pero esto no es suficiente ya que nosotros nos encontraremos caras en diferentes orientaciones y el clasificador solo está entrenado con caras rectas. 

La solución es rotar en diferentes ángulos la imagen hasta que detectemos cara. Después de muchas pruebas decidí girar la imagen con los siguientes ángulos: 

[45, 90, 135, 180, 225, 270, 315, 360]

Probé a hacerlo con 90, 180, 270 y 360 pero en muchas ocasiones no era capaz de detectar algunas de las caras. 

Esta parte de rotar puede parecer fácil, ya que es simplemente aplicar una matriz de rotación a la imagen. Pero me encontré con el problema de que al rotar la imagen respecto su centro me recortaba la imagen:

Mi solución ha sido ponerle un marco a la imagen de 40 pixeles a cada lado de la imagen, de esta manera no de recorta información importante:

Como vemos, gracias a los ángulos añadidos y el marco que le he puesto es capaz de detectar la cara hasta en 2 ocasiones.

Otros problemas con los que he tenido que lidiar es que el detector te da la esquina superior izquierda del rectángulo de detección, pero claro te la da respecto la imagen rotada en la que ha detectado la cara. Solución, aplicar una matriz de rotación al punto con -ángulo_con_el_que_se_detectó. 

Segundo problema, si pretendes dibujar el rectángulo de detección con cv2.rectangle() este te lo dibujará pero recto. En los casos en los que el ángulo con el que se detecta la cara no es 90, 180, 270 o 360 querremos ver el rectángulo girado. Para ello, cojo cada esquina del rectángulo y les aplico una matriz de rotación respecto el punto ya girado de la esquina superior izquierda del rectángulo (ángulo de giro = 360 - ángulo de detección). Luego, uno esos puntos con líneas usando cv2.line(). Para saber el centro del rectángulo de detección primero lo calculo ya que sé el ancho y alto del rectángulo y posteriormente lo roto al igual que los puntos anteriores. 

Aparte de todo esto, como para poder detectar sin recortes he tenido que ponerle un marco a la imagen, para que todo se vea correctamente en la imagen original, a la esquina superior izquierda del rectángulo, tras rotarlo, le resto tanto en x como en y el marco añadido. 

• Pasar de pixel a coordenada Gazebo

 En el caso de que queramos subir tanto el dron como para que sea capaz de ver 2 personas al mismo tiempo, habría que:

1. Hacer un programa tal que sitúe el centro de la imagen en el centro de detección y apuntar la coordenada en Gazebo
2. Situarse en diferentes posiciones desde las cuales veamos a dicha persona y apuntar los pixeles donde la detectamos.
3. Encontrar la relación tal que:

Se calcularía la constante en x y en y, y con esto ya podrías obtener las coordenadas reales.

En el caso de que vueles tan bajo como para solo detectar a una persona a la vez, bastaría con imprimir la coordenada actual del dron y dar unas holguras en x e y para decir si es una nueva persona o no.

• ¿Cómo saber si es una nueva persona?

Para ello me guardaré en una lista las coordenadas de Gazebo dónde he visto las caras, si la distancia euclídea de la nueva coordenada de detección dista 3 metros o menos considero que es una persona ya vista y no la vuelvo a guardar, en caso contrario la añado a mi lista.

• ¿Cuándo pasar de buscar a volver a la plataforma?

En mi caso he generado una lista de puntos antes de comenzar el while True, para seguir la trayectoria de una espiral cuadrada. Esta lista no es infinita, termina cuando el lado del cuadrado alcanza un tamaño de 35 metros. Por tanto, conforme voy alcanzando los puntos los voy sacando de la lista con pop(), cuando en la lista no quedan más puntos la búsqueda termina y el dron va a la plataforma.

Cuarta etapa: Ir a la plataforma

De nuevo usaré el control en posición, restaré la coordenada de la plataforma menos la actual, si esta diferencia es menor de 0.08 (para hacer un ajuste fino) pasaré a la siguiente etapa, aterrizar.

Quinta y última etapa: Aterrizar

Esta etapa es sencilla ya que Unibotics nos proporciona una función para aterrizar llamada HAL.land(), por tanto simplemente la llamaremos y así acabará el programa.

Resultado Final

En este primer vídeo no se pudo terminar la ejecución debido a un BrokenPipe del docker:

 

En este segundo vídeo se puede ver la ejecución completa del programa, por falta de tiempo opté por volar bajo, a 2.2 metros y dar una holgura de 3 metros para decir que es nueva persona, por ello en el caso de rajoy se detecta pero no se imprime por su cercanía al político de delante: