Insertando nuevos objetos en un mundo ya creado en Gazebo

Para poder introducir nuevos objetos en Gazebo he partido del código del repositorio https://github.com/JdeRobot/TeachingRobotics/tree/master/src/global_navigation. En global_navigation teníamos el modelo yellowTaxi, el cual lo podemos ver en una imagen del mundo de Gazebo que presentaba la práctica global_navigation.

En el repositorio mencionado he modificado los archivos cityMedium.world y cityLarge.world para poder introducir los nuevos objetos. En mi caso he introducido los modelos coke_can y hokuyo. Los archivos modificados quedarían de la siguiente forma:

cityMedium.world

<?xml version="1.0" ?>
<sdf version="1.4">

    <world name="cityMedium">
        <!-- My city -->
        <include>
            <uri>model://cityMedium</uri>
        </include>
        <!-- My robots -->
        <include>
           <uri>model://yellowTaxi</uri>
           <pose>0 0 0.5 0 0 0</pose>
        </include>
        <!-- A global light source -->
       <include>
           <uri>model://sun</uri>
       </include>
       <include>
          <static>true</static>
          <uri>model://coke_can</uri>
          <pose>4.5 0 0.75 0 0 0</pose>
          <box>
              <size>2 2 0.01</size>
          </box>
       </include>
      <include>
          <uri>model://hokuyo</uri>
          <pose>3 14 0.9 0 0 0</pose>
          <box>
              <size>20 20 0.4</size>
          </box>
     </include>
  </world>
</sdf>

cityLarge.world

<?xml version="1.0" ?>
<sdf version="1.4">

    <world name="cityLarge">
        <!-- My city -->
        <include>
            <uri>model://cityMedium</uri>
        </include>
        <!-- My robots -->
        <include>
           <uri>model://yellowTaxi</uri>
           <pose>0 0 0.5 0 0 0</pose>
        </include>
        <!-- A global light source -->
       <include>
           <uri>model://sun</uri>
       </include>
       <include>
          <static>true</static>
          <uri>model://coke_can</uri>
          <pose>4.5 0 0.75 0 0 0</pose>
          <box>
              <size>2 2 0.01</size>
          </box>
       </include>
      <include>
          <uri>model://hokuyo</uri>
          <pose>3 14 0.9 0 0 0</pose>
          <box>
              <size>20 20 0.4</size>
          </box>
     </include>
  </world>
</sdf>


En la siguiente imagen podemos ver los nuevos objetos que hemos incluido en el mundo de la práctica global_navigation.

Práctica 3: global navigation

En esta práctica el objetivo es implementar la lógica del algoritmo de navegación GPP (Gradient Path Planning). El objetivo es alcanzar un destino marcado en un mapa, para ello emplearemos la navegación global mediante el algoritmo GPP. Este algoritmo consiste en que una vez marcado el destino, se irá expandiendo un campo imaginario desde dicho destino hasta que llegue un poco más allá del robot, para dejar un margen en el cálculo del campo. El valor de dicho campo irá aumentando con la distancia al destino. Además tendremos que tener en cuenta en la expansión del campo que habrá puntos que sean camino y puntos que sean obstáculo, para ello se nos proporciona una imagen del mapa de la ciudad por donde tiene que circular el coche, (que se puede obtener con la función grid.getMap()), donde los píxeles con valor 255 son camino y los píxeles con valor 0 son obstáculo. La imagen del mapa proporcionado la podemos ver a continuación:

Para realizar esta práctica emplearemos la plataforma JdeRobot (nos basamos en el código de https://github.com/JdeRobot/TeachingRobotics/tree/master/src/global_navigation), el simulador Gazebo, la librería OpenCV y como lenguaje de programación Python. Para poder evaluar el campo dividiremos el proceso en dos partes: la expansión del campo, sin tener en cuenta las penalizaciones por obstáculos; y las penalizaciones por obstáculos.

Empezaremos primero con la expansión del campo. Para ello debemos saber que a las celdillas de los obstáculos les pondremos un valor muy elevado para que a la hora de calcular la ruta más corta no se elijan las celdillas con obstáculo. El campo se irá expandiendo como si de frentes de onda se tratara, el primer frente de onda se iniciará en el destino. Para propagar el frente de onda, tenemos que tener en cuenta que nos proporcionan un grid donde iremos anotando los valores con la función grid.setVal(x, y, val). Como al realizar la expansión del campo partimos del destino, entonces tendremos que tener en cuenta que al destino si es una celdilla del camino le pondremos valor 0 y si es obstáculo le pondremos un valor muy alto, por ejemplo 20000. Para calcular el valor de las celdillas vecinas, tendremos que tener en cuenta que en la primera iteración el destino pondrá valor a sus 8 vecinos, por lo que de estos 8 vecinos las celdillas que se encuentren en diagonal con respecto al destino, les pondremos un valor igual al valor de la celdilla del destino más raíz cuadrada de dos, y al resto les pondremos un valor igual al valor de la celdilla del destino más 1. En la siguiente iteración los 8 vecinos del destino querrán propagarse también, por lo que para cada uno de estos 8 vecinos evaluaremos el valor de sus 8 vecinos correspondientes. Para ello, tendremos que ver si la celdilla a la que queremos propagar el campo, ya tenía un valor, si es así decidiremos poner el valor más pequeño. Además seguiremos la misma lógica que en el caso del destino, si nos encontramos en celdillas que están situadas en la diagonal, debemos aumentar el campo en raíz cuadrada de 2, y si están contiguas en otra posición aumentaremos en 1 el valor del píxel central. Si en la casilla que estamos evaluando tiene un valor nan, un valor negativo o el valor es cero (pero la celdilla no coincide con el destino), entonces tendremos que ponerle un valor a esa celdilla, ya que esto significa que ningún píxel anterior había propagado su campo a esta celdilla. Es decir, si consideramos hasta la expansión del tercer frente de onda y que todas las celdillas forman parte del camino, los valores del campo quedarían de la siguiente forma:

Los pasos que debemos seguir para la expansión del campo sin tener en cuenta la penalización de obstáculos es la siguiente:

1. Primero obtenemos la imagen del mapa con la función grid.getMap() del objeto grid (que es nuestra rejilla donde apuntamos los valores). Además, empleamos la función grid.getDestiny() que devuelve el destino seleccionado en la GUI; y la función grid.getPose(), que nos devuelve la posición respecto al mapa.

mapIm = self.grid.getMap()

dest = self.grid.getDestiny()

gridPos = self.grid.getPose()

2. Obtenemos la posición del robot con las funciones sensor.getRobotX() y sensor.getRobotY(). Tenemos que tener en cuenta que dichas funciones nos devuelven la posición respecto al mundo en Gazebo, por lo que tendremos que emplear la función grid.worldToGrid(worldX, worldY) para pasar dicha coordenada a la coordenada respectiva en el mapa. Esta coordenada la emplearemos para calcular hasta dónde generar la expansión en cada caso.

# Position of the robot

world_robotX = self.sensor.getRobotX()

world_robotY = self.sensor.getRobotY()

posRobot =self.grid.worldToGrid(world_robotX, world_robotY)

3. Nos creamos algunas variables necesarias para la expansión del campo y las inicializamos (en la variable margen se podría poner cualquier otro valor):

fin = "false"

square = 0

margin = 30

4. Evaluamos el valor de la posición del destino, es decir, si pertenece al camino le pondremos un valor de 0, y si es obstáculo le pondremos  un valor de 20000:

if (mapIm[dest[1]][dest[0]] == 255):

self.grid.setVal(dest[0], dest[1], 0.0)

else:

                                       self.grid.setVal(dest[0], dest[1], 20000.0)

 5. Creamos un bucle while para realizar la expansión del campo, mientras fin sea false. La variable fin la evaluamos en cada iteración para marcar un fin, ya que no nos interesa que calcule el campo de toda la imagen, ya que sería más lento, nos interesa que se calcule el campo hasta un poco más allá de la posición del robot, esto lo miramos con la función findStopExpansion(dest, posRobot, margin, i, j, fin), que me he creado para ver si ya se ha superado la posición del robot dejando un margen. En dicha función evaluamos diferentes casos porque dependerá de en que situación esté el destino para ver hasta donde se expande el campo. Esta función es la siguiente:

def findStopExpansion(self, dest, posRobot, margin, i, j, fin):

# Cases of the margins

if ((dest[0] <= posRobot[0]) and (dest[1] <= posRobot[1])):

if((i > (posRobot[0] + margin)) and (j > (posRobot[1] + margin))):

fin = "true"

elif ((dest[0] >= posRobot[0]) and (dest[1] <= posRobot[1])):

if((i < (posRobot[0] - margin)) and (j > (posRobot[1] + margin))):

fin = "true"

elif ((dest[0] >= posRobot[0]) and (dest[1] >= posRobot[1])):

if((i < (posRobot[0] - margin)) and (j < (posRobot[1] - margin))):

fin = "true"

elif ((dest[0] <= posRobot[0]) and (dest[1] >= posRobot[1])):

if((i > (posRobot[0] + margin)) and (j < (posRobot[1] - margin))):

fin = "true"

return fin

El bucle while lo podemos ver a continuación:

        while (fin == "false"):
            for i in range(dest[0]-square, dest[0]+square+1):
                for j in range(dest[1]-square, dest[1]+square+1):
                    if ((i >= 0) and (i < 400) and (j >= 0) and (j < 400)):
                       for k in range(i-1, i+2):
                           for l in range(j-1, j+2):
                               if ((k >= 0) and (k < 400) and (l >= 0) and (l < 400)):
                                   val = self.grid.getVal(k, l)
                                   val_init = self.grid.getVal(i, j)
                                   if ((k == dest[0]) and (l == dest[1])):
                                       self.grid.setVal(k, l, self.grid.getVal(k, l))
                                   elif ((k == i) and (l == j)):
                                       self.grid.setVal(k, l, val_init+0.0)
                                   else:
                                       if ((k != i) and (l != j)):
                                                 if ((math.isnan(val)) or ((val_init + math.sqrt(2.0)) < val) or (val <= 0)):
                                               self.grid.setVal(k, l, val_init+math.sqrt(2.0))
                                       else:
                                           if ((math.isnan(val)) or ((val_init + 1.0) < val) or (val <= 0)):
                                               self.grid.setVal(k, l, val_init+1.0)
                                       if (mapIm[l][k] == 0):
                                           val_pos = self.grid.getVal(k, l)
                                           self.grid.setVal(k, l, (val_pos+20000.0))
                    # Cases of the margins
                    fin = self.findStopExpansion(dest, posRobot, margin, i, j, fin)
            square = square + 1

En este bucle podemos ver que se realiza un for para ver desde donde hasta donde se realiza el frente de onda, que irá aumentando con square, ya que primero realizaremos un único frente de onda (el del destino), luego otro frente de onda, y así continuamente. La condición que ponemos de 0<=i<400 y 0<=j<400, es porque el tamaño de la imagen es de 400x400, entonces no queremos que se evalúen posiciones que no existen en la imagen. A continuación tenemos dos for que nos servirán para evaluar el valor de los 8 vecinos de cada celdilla del frente de onda. En el siguiente if pasa lo mismo que en la condición anterior, es decir, no queremos evaluar posiciones inexistentes en la imagen. Empleamos la función grid.getVal(x, y) para que nos devuelva el valor de la posición (x, y) en el grid. Por eso empleamos las variables val y val_init para ir viendo el valor de la posición en la celdilla, y el valor de la celdilla perteneciente al frente de onda más raíz cuadrada de dos o más uno, dependiendo del caso en el que nos encontremos. Tenemos que tener en cuenta que si estamos evaluando una celdilla de obstáculo le tendremos que sumar un valor de 20000, por lo que tendremos que mirar el valor que tenemos en mapIm en dicha posición. Para ello es importante saber que las coordenadas x,y están invertidas en el mapa respecto al grid, por eso en la condición "mapIm[l][k] == 0" no hemos puesto "mapIm[k][l] == 0".

Además tenemos que calcular la ruta más corta y dibujarla en verde para ver que se está realizando bien la expansión del campo. Para calcular la ruta más corta, he creado unas variables que me sirvan para realizar un bucle y calcular el camino más corto. Me creo las siguientes variables y las inicializo:

pixelCentral = [posRobot[0], posRobot[1]]

valMin = self.grid.getVal(posRobot[0], posRobot[1])

posMin = pixelCentral

self.grid.setPathVal(posRobot[0], posRobot[1], valMin)

found ="false"

 
En pixelCentral nos guardamos la posición del robot, esto nos servirá porque la ruta más corta la iremos calculando a partir de la posición del robot. Lo que haremos es mirar los 8 vecinos del pixelCentral y nos quedaremos con el vecino que tenga un valor más pequeño en el grid, que en la próxima iteración será el pixelCentral y comprobaremos para este píxel sus 8 vecinos para elegir la celdilla con menor valor. Así iremos construyendo el camino más corto desde el robot al destino. Para ello emplearemos la función grid.setPathVal(x,y, val), que establece el valor val en la posición indicada, y además utilizaremos la función grid.setPathFinded(), que establece que ya se ha encontrado el camino para que pueda empezar a pintarse. El bucle lo podemos ver a continuación:

while (found == "false"):

FoundNeighbour = "false"

for i in range(pixelCentral[0]-1, pixelCentral[0]+2):

for j in range(pixelCentral[1]-1, pixelCentral[1]+2):

if ((i >= 0) and (i < 400) and (j >= 0) and (j < 400)):

if (FoundNeighbour == "false"):

valMinNeighbour = self.grid.getVal(i, j)

posMinNeighbour = [i, j]

FoundNeighbour = "true"

val = self.grid.getVal(i, j)

if ((val <= valMin) and (val >= 0.0)):

if (((val == 0.0) and (i == dest[0]) and (j == dest[1])) or (val > 0.0)):

valMin = val

posMin = [i, j]

valMinNeighbour = valMin

posMinNeighbour = posMin

elif (val < valMinNeighbour):

valMinNeighbour = val

posMinNeighbour = [i, j]

self.grid.setPathVal(posMinNeighbour[0], posMinNeighbour[1],    valMinNeighbour)

pixelCentral = posMinNeighbour

if ((valMinNeighbour == 0.0) and (posMinNeighbour[0] == dest[0]) and        (posMinNeighbour[1] == dest[1])):

found = "true"

         self.grid.setPathFinded()



A continuación podemos ver una imagen del camino más corto sin tener aún las penalizaciones de obstáculos:

En la imagen se puede ver el gradiente del campo donde pone Grid Field (esto es tras elegir el destino en introrob.py haciendo click en un punto, y tras haber dado al botón Generate Path), y en donde pone introrob.py podemos ver una cruz roja del destino que hemos elegido y el trazo verde se corresponde con el camino más corto encontrado. Vemos en el camino que al no haber añadido aún las penalizaciones de los obstáculos a las celdillas más cercanas, la línea pasa rozando el borde de los obstáculos, por eso luego habrá que añadir dichas penalizaciones, para poder aumentar el valor de dichas celdillas y que al realizar el cálculo de la ruta más corta no se elijan dichas celdillas, esto se debe a que si no fuera así al realizar la navegación del coche, éste se rozaría con las paredes de los obstáculos.


El código se encuentra en el siguiente enlace: https://github.com/RoboticsURJC-students/2016-tfg-vanessa-fernandez/blob/master/Practices%20of%20robotics/global_navigation/MyAlgorithm.py

Práctica 2: Obstacle avoidance

En esta práctica el objetivo es implementar la lógica del algoritmo de navegación VFF (Virtual Force  Field). El principio de esta técnica es que cada objeto del entorno del robot genera una fuerza repulsora hacia el robot, y el destino genera una fuerza atractora en el Fórmula1. El fórmula1 se moverá en la dirección de la suma de las dos fuerzas, de esta forma se podrán evitar obstáculos, además de llegar al destino. Para poder llegar a un destino remoto, utilizaremos  la navegación local, para la cual tendremos subojetivos a los que llegar.

Para poder implementar esta práctica disponemos del simulador Gazebo y la plataforma JdeRobot. Además el Fórmula1 tendrá un sensor láser (que nos permite medir distancias en un plano, proporcionándonos un array de 180 números), además de las cámaras derecha e izquierda; y como actuadores tendremos los motores, para los cuales tendremos en cuenta la velocidad de tracción y la velocidad de rotación.

En la siguiente imagen podemos ver un manto azul que son las lecturas del láser, además tenemos un vector verde, que es el vector atractor; un vector rojo, que es el vector repulsor; y por último un vector negro, que es la combinación de los dos anteriores.

 

En la práctica tenemos los siguientes pasos:

1.  Obtenemos la posición del subobjetivo al que deseamos llegar:

        self.currentTarget=self.getNextTarget()
        self.targetx = self.currentTarget.getPose().x
        self.targety = self.currentTarget.getPose().y

 2. Obtenemos la posición del robot y la orientación del robot respecto al mapa:

        rx = self.sensor.getRobotX()
        ry = self.sensor.getRobotY()

        rt = self.sensor.getRobotTheta()

3. Marcamos los subobjetivos por los que ya hemos pasado:

        if(abs(ry)<(abs(self.targety)+1) and abs(ry)>(abs(self.targety)-1)):
            self.currentTarget.setReached(True)

4. Obtenemos los datos del sensor láser, que consiste en 180 pares de valores y obtenemos los datos del láser de una forma más clara:

        laser_data = self.sensor.getLaserData()
        laser = parse_laser_data(laser_data)

5. Convertimos las coordenadas absolutas del subobjetivo en coordenadas relativas al robot:

        self.carx,self.cary=absolutas2relativas(self.targetx,self.targety,rx,ry,rt)

 6. Con los datos del láser, obtenemos el vector repulsor:

        dist_threshold = 6
        vff_repulsor_list = []
        for d,a in laser:
            # (4.2.1) laser into GUI reference system
            if(d < dist_threshold):
                x = (d - dist_threshold) * math.cos(a) * -1
                y = (d - dist_threshold) * math.sin(a) * -1
                v = (x,y)
                vff_repulsor_list += [v]

        vff_repulsor = np.mean(vff_repulsor_list, axis=0)

        self.obsx,self.obsy = vff_repulsor

7. Calculamos el módulo del vector repulsor y en función de su valor se queda como está o  aumentamos su peso:

        mod_repulsor = pow(pow(self.obsx,2) + pow(self.obsy,2),0.5)
        if (mod_repulsor > 1.55):
            self.obsx,self.obsy = vff_repulsor * 4.5

8. Hacemos la suma del vector atractor y del vector repulsor para obtener el vector resultante:

        self.avgx = self.carx + self.obsx
        self.avgy = self.cary + self.obsy
 

9. Cálculo del módulo de la velocidad:

         speed = pow(pow(self.avgx,2) + pow(self.avgy,2),0.5)

10. Cálculo de la corrección:

        if (abs(self.obsx) > 2):
            if (abs(self.obsx) < abs(self.carx)):
                if (self.obsx >= 0):
                    self.avgx = abs(self.avgx)
                else:
                    self.avgx = -abs(self.avgx)

        if ((self.obsx == (-self.carx)) and (self.obsy == (-self.cary))):
            self.avgx = self.obsx
            self.avgy = self.cary

11. Calculamos el ángulo:

        if (speed < 1):
            # Use the tangent to avoid indeterminacy
            angle = math.atan(abs(self.avgx/self.avgy))
        else:
            angle = math.asin(abs(self.avgx/speed))
        if(self.avgy > 0):
            angle = math.pi - angle 

12. Asignamos las velocidades de tracción y de rotación:

# Linear speed
        if ((speed < 1) or (speed > 3)):
            self.sensor.setV(3)
        else:
            self.sensor.setV(speed)

        # Angular speed
        if(self.avgx < 0):
            self.sensor.setW(angle * 0.75)
        else:
            self.sensor.setW(-angle * 0.75)

 A continuación podemos ver un vídeo de la práctica:

 

El enlace con el código de la práctica es el siguiente:  https://github.com/RoboticsURJC-students/2016-tfg-vanessa-fernandez/tree/master/Practices%20of%20robotics/obstacle_avoidance

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