Práctica 1: follow line

En esta práctica emplearemos el simulador Gazebo, la plataforma Jderobot (nos basamos en el código de https://github.com/JdeRobot/TeachingRobotics/tree/master/src/follow_line) y la librería OpenCV para procesamiento de imagen. Para poder utilizar la librería OpenCV tendremos que importarla con import cv2.

La práctica consiste en realizar un control reactivo PID y completar una vuelta del circuito Fórmula 1. Para ello nuestro fórmula 1 constará de dos sensores, que son dos cámaras, una izquierda y una derecha; además tendremos dos actuadores: la velocidad de tracción y la velocidad de rotación. Las cámaras nos proporcionarán información de los píxeles como una matriz de números, que nos servirá para guiar a nuestro fórmula1. El circuito lo podemos ver en la siguiente imagen:

Los pasos que debemos seguir son los siguientes:

1. Conseguir las imágenes de las dos cámaras (estas imágenes estarán en el espacio de color RGB):

        imageLeft = self.sensor.getImageLeft()
        imageRight = self.sensor.getImageRight()

 2. Para poder filtrar la imagen y quedarnos únicamente con la carretera, tendremos que transformar primero las imágenes al espacio de color HSV:

        imageRight_HSV = cv2.cvtColor(imageRight, cv2.COLOR_RGB2HSV)
        imageLeft_HSV = cv2.cvtColor(imageLeft, cv2.COLOR_RGB2HSV)

3.  Después tendremos que elegir un valor mínimo y uno máximo para poder filtrar el resto de la imagen y quedarnos solamente con la carretera:

        value_min_HSV = np.array([0, 235, 60])
        value_max_HSV = np.array([180, 255, 255])

4. Filtramos las imagenes obteniendo unas imágenes binarias:

        imageRight_HSV_filtered = cv2.inRange(imageRight_HSV, value_min_HSV, value_max_HSV)
        imageLeft_HSV_filtered = cv2.inRange(imageLeft_HSV, value_min_HSV, value_max_HSV)

5.  Las imágenes que obtenemos con la función anterior, únicamente tienen un canal, por lo que para mostrar las imágenes tendremos que emplear la siguiente función de tres canales, para la cual empleamos tres veces la misma imagen:

        imageRight_HSV_filtered_Mask = np.dstack((imageRight_HSV_filtered, imageRight_HSV_filtered, imageRight_HSV_filtered))
        imageLeft_HSV_filtered_Mask = np.dstack((imageLeft_HSV_filtered, imageLeft_HSV_filtered, imageLeft_HSV_filtered))

Mostramos las imágenes:

        #SHOW THE FILTERED IMAGE ON THE GUI
        self.setRightImageFiltered(imageRight_HSV_filtered_Mask)
        self.setLeftImageFiltered(imageLeft_HSV_filtered_Mask)

A continuación vemos las imágenes de la cámara derecha y de la izquierda tanto en color como en binario:

6. Con la imagen binaria, que tiene en blanco la carretera y en negro el resto, ya podemos analizar los puntos de interés de la imagen. Primero debemos encontrar los dos puntos extremos de la línea roja:

        position_pixel_left = []
        position_pixel_right  = []

        for i in range(0, columns-1):
            value = imageLeft_HSV_filtered[365, i] - imageLeft_HSV_filtered[365, i-1]
            if(value != 0):
                if (value == 255):
                    position_pixel_left.append(i)
                else:

                    position_pixel_right.append(i-1)

En el código anterior vemos que miramos la fila 365 y tendremos que añadir al array de la izquierda el número de la columna donde value (resta del píxel actual y el píxel anterior) es 255, y al array derecho le añadiremos el número de la columna donde value es -255.

7. Tendremos que comprobar que se rellenan los dos arrays para que no haya un problema de ejecución. De estar algún array vacío lo rellenamos y además calcularemos la posición media de la carretera:

        if ((len(position_pixel_left) != 0) and (len(position_pixel_right) != 0)):
            position_middle = (position_pixel_left[0] + position_pixel_right[0]) / 2
        elif ((len(position_pixel_left) != 0) and (len(position_pixel_right) == 0)):
            position_middle = (position_pixel_left[0] + columns) / 2
        elif ((len(position_pixel_left) == 0) and (len(position_pixel_right) != 0)):
            position_middle = (0 + position_pixel_right[0]) / 2
        else:
            position_pixel_right.append(1000)
            position_pixel_left.append(1000)
            position_middle = (position_pixel_left[0] + position_pixel_right[0])/ 2

8. Calcularemos la desviación de nuestro coche:

        desviation = position_middle - (columns/2)

9. Hacemos un control P, para ello empleamos diferentes velocidades de tracción y de rotación:

        if (desviation == 0):
             self.sensor.setV(10)
        elif (position_pixel_right[0] == 1000):
             self.sensor.setW(-0.0000035)
        elif ((abs(desviation)) < 85):
             if ((abs(desviation)) < 15):
                 self.sensor.setV(6)
             else:
                 self.sensor.setV(3.5)
             self.sensor.setW(-0.000045 * desviation)
        elif ((abs(desviation)) < 150):
             if ((abs(desviation)) < 120):
                 self.sensor.setV(1.8)
             else:
                 self.sensor.setV(1.5)
             self.sensor.setW(-0.00045 * desviation)
        else:
             self.sensor.setV(1.5)
             self.sensor.setW(-0.005 * desviation)

El vídeo de la práctica es el siguiente:

El código está en el siguiente enlace:

https://github.com/RoboticsURJC-students/2016-tfg-vanessa-fernandez/tree/master/Practices%20of%20robotics/follow_line