Nodo de exploración BFS + mapa de segmentos
Exploración BFS por fronteras con planificación Dijkstra/A*
Esta semana estuve trabajando en el nodo de exploración BFS para que el Rover pudiera completar la exploración del almacén y construir un mapa de segmentos aceptable.
Primera aproximación: ir directo a la frontera
La primera versión del nodo era muy simple: el robot detectaba una celda frontera (zona libre pegada a zona desconocida), rotaba hasta orientarse hacia ella y avanzaba en línea recta.
El problema de este enfoque es que, cuando el mapa ya estaba parcialmente construido, muchas fronteras quedaban detrás de obstáculos (estanterías, pallets, etc.). El robot se quedaba bloqueado porque no era capaz de rodear los obstáculos ni buscar un camino alternativo.
Segunda aproximación: planificar un camino hasta la frontera
Para resolverlo, cambié el comportamiento: en vez de moverse en recto hacia la frontera, el nodo calcula un path sobre la rejilla usando Dijkstra. Así, el robot puede encontrar un camino válido por los pasillos y llegar a la frontera aunque haya obstáculos entre medias.
El funcionamiento del nodo es el siguiente:
Construye mapa interno tipo Occupancy Grid a partir del LIDAR. Las celdas tienen el valor:
- UNKNOWN: no observada todavía
- FREE: observada y libre
- OCC: observada y ocupada
Infla los obstáculos para que planifique con un margen de seguridad.
Detecta fronteras. Éstas son las celdas FREE que tienen al menos una vecina UNKNOWN
Elige la mejor frontera y construye el camino.
Convierte el camino en waypoints en coordenadas mundo.
El robot se mueve por cada waypoint hasta el goal.
Además, el robot replanifica si se dan condiciones complicadas para llegar al objetivo durante varios ciclos:
1
2
3
4
- La frontera se ha vuelto ocupada.
- El waypoint actual queda bloqueado.
- No hay progreso a la celda objetivo durante demasiado tiempo.
- El camino hacia un waypoint aparece bloqueado.
Si se dan estas condiciones, se limpia el goal y se vuelve al paso 3.
En RVIZ se publica el Occupancy grid y marcadores con la celda del robot, la celda frontera objetivo y el path a seguir para depurar.
El robot estuvo casi media hora dando vueltas por el almacén, obteniendo un resultado bastante bueno que podemos ver en el siguiente vídeo:
