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.

De lo que ya viene resuelto, hay una línea que conviene mirar: las dos suscripciones de evasor.py están una al lado de la otra y son distintas a propósito — /scan va con qos_profile_sensor_data y /odom con 10. Es el primer workshop que toca un sensor de verdad, y ese detalle es la diferencia entre recibir datos y no recibir nada. Está explicado abajo, en El QoS de los sensores.

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.

Explicación: el QoS de los sensores

En la semana 01 las suscripciones se crearon con create_subscription(String, 'mensaje', self.recibir, 10). Ese 10 es el tamaño de cola: cuántos mensajes sin procesar guarda ROS 2 antes de descartar los más viejos. Pero pasar solo un número también elige, sin que se note, el resto del perfil de QoS (Quality of Service): las políticas que publisher y subscriber negocian para poder conectarse. La que importa acá es reliability, y el default de rclpy es Reliable — reintentar hasta que el mensaje llegue.

Para un String a 1 Hz está perfecto. Los sensores no funcionan así: el lidar publica a unos 7 Hz y no va a parar nunca, así que si un scan se pierde, reintentarlo no tiene sentido — ya viene el próximo, y es más nuevo. Por eso los sensores publican en Best Effort, tanto en el simulador como en el ROSMASTER X3 físico.

Un subscriber Reliable no se conecta a un publisher Best Effort. DDS considera los perfiles incompatibles y directamente no arma la conexión. No es un error, no es una excepción, no se cae nada: el callback simplemente no se llama nunca. El robot se queda quieto y la pantalla no dice nada.

Por eso evasor.py suscribe el /scan con el perfil de sensores, que rclpy ya trae armado:

from rclpy.qos import qos_profile_sensor_data
...
self.create_subscription(LaserScan, 'scan', self.recibir_scan, qos_profile_sensor_data)

La regla no es “los sensores siempre en Best Effort”, es que el subscriber tiene que matchear con el publisher del otro lado, sea cual sea. /odom lo publica wheel_state_odometry, un nodo común, en Reliable, y por eso va con 10. Para averiguar con qué QoS publica alguien:

ros2 topic info /scan --verbose

Y si un nodo no recibe nada pero ros2 topic hz muestra el tópico publicando perfecto, buscá esto en el log — es la única pista que da ROS 2:

[evasor] New publisher discovered on topic 'scan', offering incompatible QoS.
         No messages will be received from it. Last incompatible policy: RELIABILITY

En la semana 05 vuelve a aparecer lo mismo, pero del lado de los displays de RViz.