08 · Cobertura del mapa

La práctica

Antes de empezar

1. 01

   Semana 06 funcionando

   Necesitás tu paquete de la semana 06 (localizador + campo_verosimilitud) corriendo — este workshop reusa el mismo patrón de paquete propio y depende de tener pose en map. No depende de semana 07: son ramas hermanas, las dos parten de la 06.

2. 02

   Un agregado a localizador.py

   Esta semana necesita que tu filtro escuche initialpose para resembrar las partículas cuando marcás la pose en RViz. No es un TODO — te lo damos resuelto, ver el paso siguiente.

3. 03

   Chequeo

   Confirmá que tu RViz tiene la herramienta "2D Pose Estimate" en la barra de arriba — si no la usaste hasta ahora, puede que no esté agregada.

Concepto mínimo

Cobertura por rayo, no por radio

La forma más simple de llevar la cuenta de “por dónde ya pasé” sería un círculo de radio fijo alrededor del robot. Es tentador porque es una sola cuenta de distancia por celda, pero ignora las paredes: si el robot pasa cerca de una víctima con una pared en el medio, un círculo la marca “cubierta” igual, aunque el robot nunca haya tenido línea de vista hacia ella.

La alternativa es preguntar, para cada rayo del /scan, qué celdas atravesó realmente — respetando paredes por construcción. Trazar esa línea sobre una grilla es el algoritmo de Bresenham: dadas una celda de origen y una de destino, devuelve todas las celdas que la línea recta entre las dos atraviesa, sin huecos ni celdas salteadas. Es, literalmente, la misma cuenta que hace por dentro cualquier algoritmo de mapeo por grilla de ocupación (SLAM incluido).

Este workshop maneja el mapa a dos resoluciones a propósito: una grilla gruesa para llevar la cuenta de qué zona ya se visitó (agrupar celdas la hace más rápida de cubrir del todo), y el mapa original, a resolución fina, para todo lo que tiene que ver con no chocar — la grilla gruesa nunca decide por dónde puede pasar el robot, solo qué zona visitar.

A* con costmap inflado

Un A* que solo distingue libre/ocupado encuentra el camino más corto — que casi siempre roza el borde de una pared, porque geométricamente eso es válido aunque no tenga ningún margen. La solución estándar (la misma que usa el costmap de Nav2) es inflar el mapa antes de planificar: un filtro duro descarta toda celda a menos del radio del robot (más un margen) de una pared, y un costo suave por encima de eso hace que moverse cerca de una pared salga más caro que moverse lejos — así A* prefiere el camino centrado cuando hay margen, sin prohibir pasar más cerca cuando es la única opción.

Por qué importa el filtro duro en particular: en el laberinto de abajo hay un atajo, una rendija de una sola celda. A* sin costmap la toma porque para un punto es un camino válido y más corto — pero un círculo del tamaño real del robot queda trabado justo en la rendija, el plan existe sobre el papel pero el robot no lo puede ejecutar. Con el filtro duro, esa celda queda directamente afuera del grafo antes de planificar.

Por qué el robot espera a que un humano le diga dónde está

El robot real no arranca sabiendo su posición en el mapa. La herramienta “2D Pose Estimate” de RViz publica esa pose inicial en initialpose, y el robot se queda quieto hasta recibirla — y todavía un rato más (tiempo_asentamiento_s), para que el filtro de partículas tenga tiempo de converger antes de sumar error de movimiento al error de localización.

La máquina de estados de esta semana es de misión (inicializando → explorando → terminado), no de movimiento como la de semana 03 — mismas reglas de diseño (transición separada de la acción, loguear cada cambio), aplicadas a decidir en qué fase de la tarea está el robot, no cómo moverse ciclo a ciclo.

Implementación

Creá tu paquete

Creá tu paquete, llamado por ejemplo cobertura_mapa, y modificá los archivos correspondientes — mismo procedimiento que en semana 06/semana 07. Copiá los cuatro archivos (trazado.py, planificador.py, grilla_cobertura.py, explorador.py) al paquete, sumá los ejecutables en setup.py y las librerías que necesites en package.xml. Por último, hacé colcon build --symlink-install, otra vez, porque vas a editar los TODOs muchas veces.

El agregado a localizador.py

Tu Localizador de semana 06 necesita escuchar initialpose y resembrar las partículas alrededor de esa pose (misma cuenta que ya hace tu __init__ con pose_inicial_x/y/theta). No es parte de los TODO de esta semana — sumale este método y suscribilo en __init__:

def recibir_pose_inicial(self, msg):
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    theta = self.yaw_de_quaternion(msg.pose.pose.orientation)
    n = self.num_particulas
    dxy = self.get_parameter('dispersion_inicial_xy').value
    dtheta = self.get_parameter('dispersion_inicial_theta').value
    x0 = np.random.normal(x, dxy, n)
    y0 = np.random.normal(y, dxy, n)
    theta0 = np.random.normal(theta, dtheta, n)
    self.particulas = np.stack([x0, y0, theta0, np.full(n, 1.0 / n)], axis=1)

Parte 1 — grilla_cobertura.py

Toda la plomería está resuelta: parámetros, suscripciones, y recibir_mapa(), que arma la grilla gruesa agrupando bloques del mapa original. Quedan 2 funciones con TODO:

1. trazar_rayo(), en trazado.py — Bresenham puro, sin ROS. Se puede probar aislado antes de tocar nada del nodo.
2. marcar_cobertura() — el corazón de este nodo: transformar cada rayo del último /scan a map (mismo patrón de semana 04/07) y usar trazar_rayo() para marcar como cubiertas las celdas que atraviesa. recibir_scan() ya está resuelta y solo guarda el último mensaje (self.ultimo_scan) — la lógica corre acá, llamada por un timer a frecuencia fija, no directo desde el callback del sensor (separación sensor/decisión).

Probalo antes de meterte con explorador: sumá grilla_cobertura a tu launch, manejá el robot con teleop, y agregá en RViz un display Map apuntando a /grilla_cobertura (Color Scheme: costmap, mismo QoS transient local que /map). Vas a ver cómo quedan cada vez menos zonas sin cubrir a medida que el robot recorre el laberinto.

Parte 2 — explorador.py

También acá está resuelta toda la plomería: suscripciones, publicación de cmd_vel/objetivo_actual/camino_planificado/estado, el inflado del mapa (recibir_mapa()), el seguimiento del camino ya planificado, y el vector repulsivo del /scan como capa de seguridad secundaria. Quedan 4 piezas con TODO, de abajo hacia arriba:

1. a_estrella(), en planificador.py — A* puro, sin ROS, testeable aislado. Ya viene con la cola de prioridad y la reconstrucción del camino resueltas; falta el cuerpo que evalúa cada vecino.
2. celda_no_cubierta_mas_cercana() — de toda la grilla de cobertura, la celda libre no cubierta más cercana al robot.
3. vector_hacia() — el vector hacia un punto, en el frame del robot. Al ser mecanum, manda linear.x/linear.y directo, sin girar el chasis para apuntar primero.
4. transicionar() — los 2 if de la máquina de estados de misión: cuándo pasar de inicializando a explorando, y de explorando a terminado.

Completalas en ese orden: a_estrella() primero, porque todo lo demás depende de ella para tener sentido end-to-end.

Ejecución

Sumá grilla_cobertura y explorador a tu propio launch de semana 06 (dos Node más en la LaunchDescription, mismo patrón que ya usaste para agregar localizador y campo_verosimilitud). Dos cosas puntuales que cambian esta semana:

• Usá el mundo laberinto_simple.world sin víctimas, el mismo que ya venías usando en semana 06 — acá no se detecta nada, así que los cubos de víctimas serían ruido innecesario para la localización (ver Desafío extra si querés probarlo igual con ellas puestas).
• Agregale a tu config de RViz los displays nuevos: Map en /grilla_cobertura (Color Scheme: costmap), Marker en /objetivo_actual, Path en /camino_planificado, y la herramienta “2D Pose Estimate” en la barra de arriba — puede que tu config de semanas anteriores no la tenga; agregala con el botón “+” al lado de “Interact” si hace falta.

Con todo levantado: esperá a que Map y LaserScan se vean estables (el robot todavía no se mueve solo), usá “2D Pose Estimate” marcando dónde está Donatello, y esperá tiempo_asentamiento_s — recién ahí arranca a explorar.

Comprobación

Con el robot explorando solo:

• La grilla de cobertura se actualiza en vivo, con cada vez menos zonas sin cubrir, y camino_planificado pasa centrado por los pasillos, no pegado a las paredes.
• /estado transiciona en orden: inicializando → explorando → terminado — mirá los logs del nodo para confirmarlo.
• Cuando ya no queda ninguna zona libre sin cubrir, el robot se detiene.
• El robot nunca intenta pasar por un hueco más angosto que su propio chasis, aunque geométricamente exista un camino más corto por ahí.

Explicación

Con esto, el robot decide solo qué zona del mapa visitar y cómo llegar sin chocar, en vez de depender de que un humano le marque destinos — el mismo problema que resuelve NavigateToPose/explore_lite de Nav2 de fábrica. El A* con costmap inflado es, literalmente, cómo planifica Nav2 por dentro. Cuando llegue el workshop de Nav2, buena parte de esto se reemplaza por configuración en vez de código — pero el concepto de fondo es el mismo.

Si sigma_costo fuera muchísimo más alto, ¿el camino planificado se volvería casi recto en cualquier pasillo, o seguiría evitando los bordes incluso donde sobra margen?

Desafío extra

• Volver a la base: hoy explorador se detiene apenas termina de cubrir el mapa. Guardá la pose del robot al confirmar “2D Pose Estimate”, agregá un estado nuevo (regresando_a_base, entre explorando y terminado) y reusá el mismo mecanismo de A* + seguimiento de camino, cambiando el objetivo por esa pose guardada.
• Calibrar sigma_costo en serio: subilo y bajalo, y mirá cómo cambia la forma del camino en pasillos anchos vs. angostos.
• Reselección de objetivo: hoy explorador recalcula la celda no cubierta más cercana en cada tick de control (10 Hz). Probá fijar el objetivo hasta llegar a él en vez de recalcularlo todo el tiempo, y compará qué tan errático se ve el recorrido.
• Elegir el objetivo por costo de camino, no por distancia recta: celda_no_cubierta_mas_cercana() usa distancia en línea recta — una celda “cerca” así puede requerir un rodeo largo si hay una pared en el medio. Correr A*/Dijkstra una vez desde la pose del robot y elegir la candidata más barata por costo de camino es más preciso, pero mucho más caro de calcular en cada tick.
• Validar en el mundo _victimas: mismo par mundo/mapa que semana 07 (laberinto_simple_victimas.world + laberinto_simple.yaml). Confirmá que la cobertura y la planificación siguen funcionando con obstáculos no mapeados presentes.