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.
| 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):
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) |
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): |
| if ((dest[0] <= posRobot[0]) and (dest[1] <= posRobot[1])): |
| if((i > (posRobot[0] + margin)) and (j > (posRobot[1] + margin))): |
| elif ((dest[0] >= posRobot[0]) and (dest[1] <= posRobot[1])): |
| if((i < (posRobot[0] - margin)) and (j > (posRobot[1] + margin))): |
| elif ((dest[0] >= posRobot[0]) and (dest[1] >= posRobot[1])): |
| if((i < (posRobot[0] - margin)) and (j < (posRobot[1] - margin))): |
| elif ((dest[0] <= posRobot[0]) and (dest[1] >= posRobot[1])): |
| if((i > (posRobot[0] + margin)) and (j < (posRobot[1] - margin))): |
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]) |
| 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"): |
| 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) |
| 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)): |
| elif (val < valMinNeighbour): |
| self.grid.setPathVal(posMinNeighbour[0], posMinNeighbour[1], valMinNeighbour) |
| pixelCentral = posMinNeighbour |
| if ((valMinNeighbour == 0.0) and (posMinNeighbour[0] == dest[0]) and (posMinNeighbour[1] == dest[1])): |
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
No hay comentarios:
Publicar un comentario