03 · Evasión de obstáculos

La práctica

Avanzar.
Detectar.
Girar.

Una máquina de estados de 2 estados: Donatello avanza en línea recta hasta que el lidar detecta un obstáculo, gira un ángulo fijo, y retoma el avance — repitiendo esto por todo el mundo.

Estados

2 · Avanzar / Girar

Sensor

/scan (lidar)

Cono

60°

Choque

0.6 m

Antes de empezar

1. 01

   Workshops previos

   Necesitás el Workshop 01 (timers) y el Workshop 02 (Twist y /cmd_vel) completados — esta semana combina las dos ideas.

2. 02

   Repositorio

   Cloná o actualizá el repo de workshops dentro de tu workspace si todavía no lo hiciste.

3. 03

   Chequeo

   Con el simulador levantado, confirmá que ros2 topic hz /scan y ros2 topic hz /odom respondan con datos antes de arrancar.

Concepto mínimo: por qué una máquina de estados

El comportamiento que buscamos (avanzar, y cuando corresponda girar) tiene dos modos claramente distintos, y en cada momento el robot solo puede estar haciendo uno de los dos. La tentación, sin pensarlo como una máquina de estados, es ir agregando variables booleanas sueltas (girando, bloqueado…) y sentencias if repartidas por todo el nodo a medida que aparecen casos nuevos — funciona al principio, pero rápido se vuelve difícil de leer y de debuggear.

Pensar el problema como una máquina de estados obliga a responder dos preguntas separadas: ¿en qué estado estoy? y ¿qué hace que pase de uno a otro? Un estado representa la situación actual del sistema (acá, ESTADO_AVANZAR o ESTADO_GIRAR — excluyentes, nunca “un poco en cada uno”). Una transición es el cambio de un estado a otro, disparado por un evento, sensor o temporizador — la misma que viste arriba, en el gráfico de “La práctica”.

Para que esto funcione bien arriba de un robot conviene seguir algunas reglas de diseño:

• El loop principal va en un timer callback, no en los callbacks de los sensores — si moviéramos el robot directamente desde recibir_scan(), la frecuencia de movimiento quedaría atada a la frecuencia (impredecible) del sensor.
• Los callbacks de sensores solo actualizan variables, nunca deciden ni mueven el robot — recibir_scan() guarda el último LaserScan, recibir_odom() guarda el yaw actual. Toda la lógica vive en un único lugar.
• La función de transición está separada de la acción — decidir si el estado cambia es una cosa, actuar según el estado ya actualizado es otra.
• Sé verboso: loguear cada transición ayuda a entender en qué estado está el robot cuando el comportamiento no es el esperado.
• Visualizá, no solo loguees: además de tomar una decisión, republicá la porción de datos que usaste para tomarla en su propio topic — acá, qué rayos del lidar caen dentro del cono de detección. Verlo en RViz (semana 05) deja confirmar de un vistazo si los parámetros están calibrados como pensás.

Implementación

evasor.py ya trae armado todo lo que no es la máquina de estados en sí: los parámetros ROS, los publishers/subscribers, y algunas funciones de apoyo resueltas (normalizar_angulo(), iniciar_giro(), angulo_girado() — la trigonometría de medir cuánto giró el robot con la odometría). Quedan 4 funciones con TODO, cada una con una guía en su docstring:

1. hay_obstaculo() — la percepción: mirar el LaserScan y decidir si hay algo demasiado cerca dentro del cono frontal. Además del bool, publica en scan_cono la máscara que usó para decidir (ya resuelto), para poder verla en RViz.
2. avanzar() — un Twist que mueve el robot derecho hacia adelante.
3. girar() — un Twist que hace girar al robot en el lugar.
4. maquina_de_estados() — el corazón del workshop: la transición (cuándo pasar de AVANZAR a GIRAR y viceversa) y el despacho a avanzar() / girar() según el estado.

Completalas en ese orden: hay_obstaculo() y las dos acciones son piezas chicas y fáciles de probar por separado, antes de escribir la máquina de estados que las usa a las tres.

También hay dos TODO en los archivos de configuración: setup.py (registrar el ejecutable evasor en entry_points) y package.xml (declarar las dependencias que usa evasor.py). Sin estos dos, colcon build puede fallar o el ejecutable no va a existir aunque el código esté perfecto.

Todos los parámetros son configurables vía --ros-args -p <nombre>:=<valor>:

Ejecución

# Terminal 1 — build
cd ~/rosmaster_ws
colcon build --packages-select evasion_obstaculos
source ~/rosmaster_ws/install/setup.bash

cafe.world no es el único mundo con obstáculos para esquivar — hay otros (los maze_*, por ejemplo). Para ver cuáles hay instalados:

ls "$(ros2 pkg prefix yahboom_rosmaster_gazebo)/share/yahboom_rosmaster_gazebo/worlds/"

Podés elegir cualquiera; acá usamos cafe.world como ejemplo porque tiene muebles a distintas distancias, bueno para probar el cono de detección.

# Terminal 2 — simulador, con un mundo con obstáculos (acá, cafe.world)
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/cafe.world" \
  motion_profile:=ideal

# Terminal 3 — nuestro nodo
source ~/rosmaster_ws/install/setup.bash
ros2 run evasion_obstaculos evasor --ros-args \
  -p angulo_vision_deg:=90.0 -p distancia_choque_m:=0.6 -p angulo_giro_deg:=110.0

Esperá a ver en la Terminal 2 el mensaje de odometría publicándose antes de correr el nodo. Usamos motion_profile:=ideal (sin resbalamiento de ruedas) mientras se prueba la lógica; una vez que funciona, probar con el default (motion_profile:=stress, más realista) es un buen próximo paso.

Comprobación

En una cuarta terminal:

ros2 topic hz /scan_cono   # solo se publica desde adentro de hay_obstaculo()

Donatello debería avanzar en línea recta hasta acercarse a un obstáculo, girar el ángulo configurado, y retomar el avance — repitiendo el patrón por todo el mundo. Los logs de transición te muestran en qué estado está en cada momento.

Si el robot gira antes de tiempo o choca igual, ¿el problema está más probablemente en hay_obstaculo() (percepción) o en maquina_de_estados() (transición)? Aislá cada parte con logs para confirmarlo.

Explicación: el ángulo del lidar en Donatello

El ángulo 0° de un LaserScan es relativo al frame del sensor (laser_link), no al frente del robot. En Donatello, el lidar está montado con 180° de yaw fijo (ver lidar.urdf.xacro en yahboom_rosmaster_description), así que el 0° del scan apunta para atrás — por eso angulo_frente_deg tiene default 180.0 y no 0.0. Es un buen ejemplo de por qué conviene revisar siempre el frame de un sensor antes de asumir que sus ángulos coinciden con los del chasis.