06 · Localización

La práctica

Antes de empezar

1. 01

   Semana 05

   Necesitás tu propio paquete launch_rviz y tu config de RViz de la semana 05 — acá los vas a extender, no a rearmar de cero.

2. 02

   Workspace clonado

   yahboom_rosmaster y jar_workshops tienen que estar clonados en ~/rosmaster_ws/src (ver la guía del simulador) — esta semana creás un paquete propio adentro de jar_workshops, no editás uno que ya viene clonado.

3. 03

   Chequeo

   Levantá map_server con laberinto_simple.yaml (Paso 0 más abajo) y confirmá que ves el mapa en RViz antes de crear ningún paquete.

Concepto mínimo: por qué la odometría deriva sola

Un robot tiene (al menos) tres frames encadenados (REP 105):

odom → base_footprint ya te lo da gratis wheel_state_odometry, corriendo desde que lanzás el simulador. Lo que falta —y es el objetivo de esta semana— es map → odom: una transformada que corrige el error acumulado comparando el /scan contra el mapa conocido. Con las dos transformadas encadenadas, cualquier nodo (RViz incluido) sabe dónde está el robot en map sin saber cómo se calculó la corrección.

Un filtro de partículas lleva, en vez de una única hipótesis de pose, cientas de ellas a la vez. En cada ciclo:

1. Predicción: cada partícula se mueve según la odometría, más ruido.
2. Corrección: cada partícula “imagina” qué vería el lidar en su pose, lo compara contra el /scan real, y las que calzan mejor pesan más.
3. Resampleo: se redibuja el conjunto dándole más chances a las partículas de mayor peso — con el tiempo, la nube se concentra alrededor de la pose real.

La pose estimada es el promedio ponderado de todas las partículas.

Implementación

Paso 0: mirá el mapa antes de escribir código

No hace falta el simulador ni código propio — alcanza con map_server sirviendo el mapa que vas a usar toda la semana:

# Terminal 1 — mapa
source ~/rosmaster_ws/install/setup.bash
ros2 run nav2_map_server map_server --ros-args -p yaml_filename:="$(ros2 pkg prefix yahboom_rosmaster_gazebo)/share/yahboom_rosmaster_gazebo/maps/laberinto_simple.yaml"

# Terminal 2 — activar el mapa
source ~/rosmaster_ws/install/setup.bash
ros2 run nav2_lifecycle_manager lifecycle_manager --ros-args -p autostart:=true -p node_names:="['map_server']"

En RViz, Fixed Frame en map y un display Map apuntando a /map: blanco = libre, negro = ocupado, gris = desconocido.

Si el display Map queda gris uniforme, es QoS, no algo roto: map_server publica con durability Transient Local, y si RViz arrancó después (que va a ser casi siempre) se suscribe pero no recibe el mensaje retenido. En el panel del display, abrí QoS y poné Durability Policy en Transient Local. Vas a necesitar el mismo ajuste para /likelihood_map más abajo.

Creá tu paquete

A diferencia de las semanas 01-05, esta vez no recibís el paquete armado: ros2 pkg create lo genera de cero.

# Terminal 1
cd ~/rosmaster_ws/src/jar_workshops/semana-06-localizacion
ros2 pkg create --build-type ament_python --dependencies \
  rclpy nav_msgs sensor_msgs geometry_msgs tf2_ros \
  localizacion

Con el paquete creado:

1. Copiá campo_verosimilitud.py y localizador.py a localizacion/localizacion/.
2. En setup.py, agregá los dos ejecutables a entry_points: campo_verosimilitud = localizacion.campo_verosimilitud:main y localizador = localizacion.localizador:main.
3. En package.xml, sumá un <depend> por cada import que no sea de la lista del ros2 pkg create: numpy (ya la resolviste en semana 04) y, solo en campo_verosimilitud.py, una librería de cálculo científico que buscás vos por su paquete apt (python3-<algo>).

cd ~/rosmaster_ws
colcon build --packages-select localizacion --symlink-install
source install/setup.bash

--symlink-install importa acá: vas a editar los TODO muchas veces, y sin symlink cada cambio pediría un rebuild.

Parte 1 — campo_verosimilitud.py: el campo de verosimilitud

Comparar el lidar contra el mapa “en vivo”, rayo por rayo, para cientos de partículas es carísimo en Python puro. La solución estándar precalcula, una sola vez apenas llega el mapa, un “campo de verosimilitud”: para cada celda, qué tan probable es que un rayo del lidar termine ahí. Después, comparar un punto del scan contra el mapa es solo leer un valor de un array.

La plomería (suscripción a /map, publisher de /likelihood_map con el mismo QoS transient local) ya está resuelta. Queda una función con TODO:

• campo_de_probabilidad() — a partir de la grilla de ocupación, calcular la distancia de cada celda al obstáculo más cercano (scipy.ndimage.distance_transform_edt) y convertirla a probabilidad con una gaussiana.

Probalo solo, sin el simulador — alcanza con map_server corriendo (Terminales 1 y 2 del Paso 0) más:

# Terminal 3
source ~/rosmaster_ws/install/setup.bash
ros2 run localizacion campo_verosimilitud

Agregá un display Map en /likelihood_map (mismo ajuste de QoS que el Paso 0) y probá un Color Scheme con gradiente: deberías ver un halo difuminado creciendo alrededor de cada pared, no líneas duras.

Parte 2 — localizador.py: predicción, corrección, resampleo

• Predicción: el movimiento entre dos lecturas de /odom se descompone en rotar hacia el rumbo (rot1), avanzar (trans), rotar lo que falte (rot2) — el modelo odométrico estándar (Thrun, Probabilistic Robotics), con ruido gaussiano proporcional a cada componente.
• Corrección: para cada partícula, transformar los puntos del /scan a su pose, leer /likelihood_map ahí, y combinar esas probabilidades en un peso.
• Resampleo: redibujar las N partículas con reemplazo, proporcional al peso (resampling sistemático).

Toda la plomería está resuelta (parámetros, suscripciones, inicialización de la nube de partículas, conversión del /scan a puntos con tf2 igual que detector_scan.py de semana 04, estimación de pose por promedio, publicación de particlecloud y de tres Path — camino_odom, camino_corregido, camino_real, este último la pose exacta de Gazebo, una ventaja del simulador que no existe en el robot físico — y la transformada map → odom vía tf2_ros.TransformBroadcaster). Quedan 3 funciones con TODO, el corazón del filtro:

1. mover_particulas() — el modelo de movimiento (predicción). Probala primero: la nube debería “abrirse” al mover el robot, aunque todavía no corrija nada.
2. pesar_particulas() — el modelo de sensor (corrección) contra /likelihood_map. La más larga; el docstring la guía paso a paso.
3. remuestrear() — el resampling sistemático.

Ejecución

Antes de abrir tu RViz de semana 05, sumale los displays nuevos: Map en /map y en /likelihood_map (QoS Transient Local en ambos), PoseArray en particlecloud, y tres Path — camino_odom, camino_corregido, camino_real — cada uno con su color, guardado con File > Save Config As sobre tu config existente.

# Terminal 1 — simulador
source ~/rosmaster_ws/install/setup.bash
ros2 launch yahboom_rosmaster_bringup rosmaster_x3_sim.launch.py \
  world:="$(ros2 pkg prefix yahboom_rosmaster_gazebo)/share/yahboom_rosmaster_gazebo/worlds/laberinto_simple.world" \
  motion_profile:=ideal rviz:=false

# Terminal 2 — mapa
source ~/rosmaster_ws/install/setup.bash
ros2 run nav2_map_server map_server --ros-args -p yaml_filename:="$(ros2 pkg prefix yahboom_rosmaster_gazebo)/share/yahboom_rosmaster_gazebo/maps/laberinto_simple.yaml"

# Terminal 3 — activar el mapa
source ~/rosmaster_ws/install/setup.bash
ros2 run nav2_lifecycle_manager lifecycle_manager --ros-args -p autostart:=true -p node_names:="['map_server']"

# Terminal 4 — nuestros nodos
source ~/rosmaster_ws/install/setup.bash
ros2 run localizacion campo_verosimilitud &
ros2 run localizacion localizador

# Terminal 5 — teleop
source ~/rosmaster_ws/install/setup.bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# Terminal 6 — tu RViz de semana 05, con los displays nuevos
source ~/rosmaster_ws/install/setup.bash
rviz2 -d <ruta a tu config de semana 05>

Ctrl-C en cada terminal para bajarlo. Una vez que lo tengas probado y andando, es un buen momento para meter estas seis terminales en tu propio launch de semana 05.

rviz:=false en la Terminal 1 es importante: el launch del simulador trae su propio RViz por default, y ese no tiene los displays nuevos que necesitás acá.

Comprobación

Manejá el robot un rato por el laberinto (con giros, no solo derecho) y mirá en RViz:

• La nube de partículas (particlecloud) se abre un poco con cada movimiento y se contrae con cada /scan que corrige — se tiene que ver “respirar”.
• camino_odom se va separando de camino_real con el tiempo — así se ve el drift directamente.
• camino_corregido se mantiene pegado a camino_real todo el tiempo, a pesar de que camino_odom se siga alejando: esa es la comprobación central, el filtro corrige el drift, no solo lo acompaña.
• El /scan se mantiene alineado con las paredes del /map, sin importar cuánto tiempo lleve andando.

Si arrancás el filtro antes de que map_server esté active, /likelihood_map nunca llega y las partículas no corrigen nunca. Confirmá primero con ros2 topic echo /likelihood_map --once que el campo ya está publicado.

Explicación

Con map → odom publicada, cualquier nodo —RViz, un futuro planificador— puede preguntar “¿dónde está el robot en el mapa?” sin saber cómo se corrigió. La diferencia es que ahora sabés qué hay adentro: un conjunto de hipótesis, un modelo de cómo se mueven, y un modelo de qué tan bien explican lo que ve el lidar. Ese mismo patrón —varias hipótesis, pesarlas contra una observación, resamplear— reaparece en muchos otros problemas de robótica más allá de localización.

Desafío extra

• Localización global: hoy el filtro arranca con una nube angosta alrededor de una pose conocida (tracking, no relocalización). El mismo algoritmo sirve para localización global con un cambio: inicializar las partículas con distribución uniforme sobre las celdas libres de todo el mapa, y subir bastante num_particulas. Probalo y fijate cuánto tarda en converger.
• Remuestreo condicional (Neff): hoy se remuestrea en cada /scan. El tamaño de muestra efectivo, Neff = 1 / Σ(peso_i²) (con pesos normalizados), mide qué tan concentrado está el peso — si ya es alto, remuestrear no suma nada y tira diversidad por la borda. Modificá recibir_scan() para remuestrear solo cuando Neff cae por debajo de, por ejemplo, num_particulas / 2.