Solución de problemas del coche real

Este capítulo es el que abres cuando el coche real no hace lo que esperabas. No hace falta leerlo en orden: busca tu síntoma en la tabla, salta al caso y sigue la receta. Cada caso tiene la misma forma:

1. Cómo se ve: lo que observas desde fuera.
2. Causas, de la más a la menos probable.
3. Diagnostica: comandos exactos, qué número esperar y una prueba a mano con el coche en el aire.
4. Arréglalo: qué clave de src/robotek_hardware/config/robot.yaml o qué constante de tu código cambia, y qué efecto tiene.
5. Comprueba que quedó: la prueba que te dice que ya está.

Cuánto te puedes fiar de cada arreglo. Pongo una etiqueta en cada hecho importante:

• [coche]: el equipo lo vio o lo midió en el coche real.
• [sim]: se probó en el simulador o con pruebas sin hardware, no en el coche.
• [código]: lo deduzco leyendo el código o con cuentas. Es razonable, pero nadie lo ha comprobado en el coche. Trátalo como una hipótesis y mídelo.

Los comandos y los scripts de este capítulo están escritos a partir del código del coche y de las notas del equipo, pero no se han ejecutado en el coche real. Si algo no sale como lo cuento, dímelo.

Tres reglas antes de tocar nada

1. Primero con el coche en el aire. Apóyalo sobre una caja para que las ruedas no toquen la mesa. Casi todo lo de aquí (sentido del servo, sentido del motor, ejes del giroscopio) se comprueba sin que el coche se mueva del sitio.
2. Primeras rondas con el motor limitado. motor_max_pct en robot.yaml es el tope de potencia. Con 40 por ciento el coche del equipo completó una ronda; con 90 por ciento se estrelló [coche]. Empieza en 30 o 40 y sube de cinco en cinco.
3. Un cambio cada vez, y anota la batería. La velocidad del coche depende de la carga (caso 5). Si cambias dos cosas y la ronda mejora, no sabrás cuál fue.

Ten siempre la mano lista para parar. Una segunda pulsación del botón de la placa publica /robotek/stop: el motor se corta al instante y no vuelve hasta relanzar [coche] (se añadió porque en la primera ronda el coche chocó y no había forma de detenerlo). Ojo con el antirrebote: una segunda pulsación en menos de 1,5 s se ignora. En una ronda oficial, qué cuenta como parar el coche lo decide el reglamento y el árbitro: pregúntalo antes.

Lo que le pasó al coche del equipo

Estos son los ocho intentos con la estrategia completa del equipo en el coche real, en una tarde. Sirven de mapa: casi todos los casos de abajo salieron de aquí. Los puntos son estimaciones a partir de lo que vio el operador, sin juez.

Índice de síntomas

Herramientas que usarás todo el rato

Con el coche lanzado (ros2 launch robotek_course car.launch.py strategy:=my_open) y el entorno cargado en cada terminal:

ros2 node list                         # qué nodos viven
ros2 topic hz /robotek/imu             # a cuántos hercios llega
ros2 topic echo /robotek/state         # en qué estado está tu estrategia
ros2 topic echo /cmd_vel_raw           # lo que pide tu estrategia (angular.z > 0 es a la izquierda)
ros2 topic info -v /start              # quién publica y quién escucha, con su QoS
ros2 topic echo --field angular_velocity.z /robotek/imu

El tópico /robotek/state es el más útil. Una ronda sana pasa por WAIT_START, CALIBRATE, DETECT_DIRECTION, FOLLOW, TURN, FOLLOW… y acaba en FINISH y DONE. Dónde se queda atascado te dice a qué caso ir.

Dos scripts pequeños para medir lo que ros2 topic echo no da bien. Guárdalos en tu portátil o en el Pi y córrelos con python3, con el entorno de ROS cargado.

import math
from collections import deque

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Imu


class ImuProbe(Node):
    def __init__(self):
        super().__init__('imu_probe')
        self.angle = [0.0, 0.0, 0.0]        # giro acumulado en x, y, z (rad)
        self.recent = deque(maxlen=100)     # últimas 100 lecturas (unos 2 s)
        self.last = None
        self.count = 0
        self.create_subscription(Imu, '/robotek/imu', self.on_imu, qos_profile_sensor_data)

    def on_imu(self, m):
        w = (m.angular_velocity.x, m.angular_velocity.y, m.angular_velocity.z)
        t = m.header.stamp.sec + m.header.stamp.nanosec * 1e-9
        if self.last is not None:
            for i in range(3):
                self.angle[i] += w[i] * (t - self.last)
        self.last = t
        self.recent.append(w)
        self.count += 1
        if self.count % 25 == 0:
            mean = [sum(r[i] for r in self.recent) / len(self.recent) for i in range(3)]
            a = [math.degrees(v) for v in self.angle]
            print(f'giro acumulado x={a[0]:+7.1f} y={a[1]:+7.1f} z={a[2]:+7.1f} grados | '
                  f'media 2 s x={mean[0]:+.4f} y={mean[1]:+.4f} z={mean[2]:+.4f} rad/s')


rclpy.init()
try:
    rclpy.spin(ImuProbe())
except KeyboardInterrupt:
    pass

import math

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan


class ScanProbe(Node):
    def __init__(self):
        super().__init__('scan_probe')
        self.create_subscription(LaserScan, '/robotek/scan', self.on_scan, qos_profile_sensor_data)

    def on_scan(self, m):
        valid, inside = [], 0
        for i, r in enumerate(m.ranges):
            a = m.angle_min + i * m.angle_increment
            a = math.degrees(math.atan2(math.sin(a), math.cos(a)))
            if abs(a) > 110.0:
                continue                          # scan_adapter descarta lo de fuera de +-110 grados
            inside += 1
            if m.range_min < r < m.range_max:
                valid.append((r, a))
        if not valid:
            print('ningún rayo válido')
            return
        r, a = min(valid)
        lost = 100.0 * (inside - len(valid)) / max(1, inside)
        print(f'más cercano: {r:.2f} m a {a:+4.0f} grados | rayos dentro de +-110: {inside} | sin dato {lost:.0f} %')


rclpy.init()
try:
    rclpy.spin(ScanProbe())
except KeyboardInterrupt:
    pass

Caso 1: no pasa nada al pulsar el botón

Cómo se ve. Pulsas el botón de la placa y los LED no pasan a azul, el coche no se mueve y /robotek/state sigue en WAIT_START. (El coche del equipo no pita: el zumbador de la placa está quemado [coche].) El caso contrario también existe: el coche arranca sin que nadie pulse (ver abajo).

Causas, de más a menos probable.

1. Alguna pieza de la cadena no está viva: botón de la placa -> /rrc/button -> start_button -> /start -> tu estrategia.
2. La estrategia está corriendo, pero no recibe /start (QoS incompatible, otro ROS_DOMAIN_ID, nodo duplicado).
3. Pulsaste dos veces: la segunda pulsación es el Stop, y deja el coche parado hasta relanzar.
4. Los paquetes rrc_lite_msgs o robotek_hardware no están cargados en esa terminal.
5. El driver de la placa no puede abrir el puerto serie.

ros2 node list                        # debes ver rrc_driver, start_button, strategy, base_adapter...
ros2 topic echo /rrc/button           # pulsa K1: sale "id: 1" y "state: 1", un mensaje por pulsación
ros2 topic echo /start                # (en otra terminal, ANTES de pulsar) sale "data: true"
ros2 topic info -v /start             # quién publica (start_button) y quién escucha (strategy)
ros2 topic echo /robotek/state        # WAIT_START -> CALIBRATE -> DETECT_DIRECTION -> FOLLOW

Caso 2: se estrella nada más arrancar

Cómo se ve. El coche sale recto, no gira donde debe y choca con el muro de enfrente en los primeros 3 a 5 s. A veces sigue empujando contra el muro.

Este es el síntoma que más causas distintas tiene, así que primero mira qué dice /robotek/state y /cmd_vel_raw justo antes del golpe:

Causas, de más a menos probable.

1. Sentido equivocado. Es lo que pasó en el intento 1: el coche decidió “horario” cuando la esquina abierta era antihoraria. Siguió recto, el frente cayó de 0,99 m a 0,12 m y chocó a los 2,9 s, y siguió mandando velocidad contra el muro hasta que se le paró a mano [coche].
2. Velocidad mayor que la del modelo. Con 90 por ciento de motor y la batería cargada, el coche “iba mucho más rápido que antes” y chocó (intento 7) [coche]. Las distancias de giro se afinaron para unos 0,6 m/s como máximo.
3. Giroscopio con el signo cambiado o eje equivocado (caso 4): el control de rumbo empuja a favor del error en vez de en contra [código].
4. Calibración del sesgo con el coche en movimiento [código]. La estrategia mide el sesgo del giroscopio en los 0,3 s siguientes al Start (CALIBRATE_SECONDS). Si todavía tienes los dedos en la placa (el botón está en la placa), o el coche rueda, esas muestras no son el sesgo. Un sesgo mal medido de solo 0,02 rad/s son unos 45 grados de rumbo falso en 40 s.
5. Servo sin centrar o con el signo al revés (caso 3).
6. LiDAR rotado, espejado o ciego (caso 9): la pared de enfrente no existe para el coche, así que nunca gira.
7. Datos de sensor congelados: si el LiDAR o la IMU dejan de publicar, un nodo sin vigilante sigue ejecutando el último escenario y el coche sigue recto (el drive.py del curso desde el paso 7 frena; ver “frenos de seguridad” abajo).
8. Sale a fondo y se encabrita (caso 5): con el morro en el aire la dirección no actúa.

Caso 3: gira al lado contrario, da círculos o va hacia la isla

Cómo se ve. En vez de corregir hacia el centro del pasillo, el coche se desvía más y más hacia un muro. O gira siempre hacia un lado y dibuja un círculo. O en la esquina gira a la derecha cuando toca a la izquierda.

Hay tres signos que pueden estar invertidos y los tres dan el mismo síntoma desde fuera: el del servo, el del giroscopio y el del LiDAR. Sepáralos antes de tocar nada:

Si las tres pasan y el coche aun así gira mal, el culpable es el sentido decidido (caso 10) o tu propia lógica.

Causas del lado del servo, de más a menos probable.

1. servo_sign invertido. Con un montaje del servo nuevo o girado, la misma orden mueve las ruedas al otro lado. El equipo lo midió en el montaje anterior y estaba bien [coche]: +20 grados (1879 µs) iban a la izquierda y -20 grados (1155 µs) a la derecha, mirando desde atrás, con las ruedas rectas en 1500 µs. Pero en el montaje nuevo (21 oct 2026) el servo cambió de sitio y giró 90 grados, y esos valores no están medidos: por eso robot.yaml trae servo_measured: false y base_adapter lo avisa en cada arranque.
2. Centro (servo_center_us) desplazado. Con las ruedas rectas no sale 1500 µs. El coche se va siempre al mismo lado. Un grado de error son unos 19 µs (la ganancia es 18,97 µs por grado en un lado y 17,24 en el otro).
3. Tabla de ruedas (wheel_table_*) del montaje viejo. Con el montaje nuevo, el ángulo real de la rueda para cada ángulo de servo es otro. Con la tabla equivocada, un lado gira más que el otro (caso 6).
4. Signo cruzado en tu código. angular.z positivo es izquierda en todo el sistema. Si cambiaste un signo en drive.py, deshazlo.

ros2 run robotek_hardware servo_steer
ros2 topic pub -r 5 /steer std_msgs/msg/Float32 "{data: 0.0}"     # ruedas rectas
ros2 topic pub -r 5 /steer std_msgs/msg/Float32 "{data: 0.30}"    # unos 17 grados a la IZQUIERDA
ros2 topic pub -r 5 /steer std_msgs/msg/Float32 "{data: -0.30}"   # a la DERECHA

ros2 topic pub -r 10 /cmd_vel_raw geometry_msgs/msg/Twist "{linear: {x: 0.2}, angular: {z: 0.5}}"

Caso 4: el giroscopio está invertido o con los ejes mal

Este es el caso al que más cuesta llegar, porque todo “funciona”: llegan datos, tienen buen tamaño, el ruido es bajo. Solo que el signo, o el eje, no son los que tu estrategia supone.

Cómo se ve.

• Signo invertido. En las rectas, el coche se desvía y cada corrección empeora el desvío hasta chocar con un muro. En la primera esquina gira a un lado y no para: el rumbo que cree llevar se aleja del objetivo en vez de acercarse, así que sigue con las ruedas a tope y da vueltas [código]. Esto es lo que pasa si la RRC Lite está de canto “del otro lado” de lo que dice robot.yaml.
• Eje equivocado. El giroscopio apenas se mueve cuando el coche gira (el eje que lees es horizontal): el rumbo no cambia, la esquina nunca se da por terminada y el coche gira sin parar. Y al revés: cuando el coche cabecea o se inclina, el “rumbo” se mueve sin que el coche haya girado [código].
• Unidades. El rumbo sale 57 veces mayor o menor que el real (caso 11).

Tu estrategia lee solo angular_velocity.z de /imu (en drive.py, msg.angular_velocity.z). Del resto de la IMU no usa nada. Por eso, para el giroscopio, el único eje que importa es up_axis: el eje de la placa que apunta hacia arriba, con su signo [código].

Las tres convenciones

1. El marco del coche (ROS). x adelante, y izquierda, z arriba, mano derecha: el giro positivo alrededor de un eje es el antihorario visto desde la punta del eje. Por eso z > 0 significa girar a la izquierda. Es lo que esperan /imu y todo tu código. En reposo la aceleración marca +9,8 en z (el acelerómetro mide la reacción del suelo, hacia arriba).
2. El marco de la placa. Los ejes dibujados en la RRC Lite (en el chip MPU6050). Están fijos a la placa. Plana, z apunta arriba. De canto, apuntan a cualquier otro sitio. El driver los publica tal cual en /rrc/imu_raw.
3. El marco aeronáutico (FRD). x adelante, y derecha, z abajo. Lo usan muchas hojas de datos, tutoriales de drones y librerías de IMU. En él un giro positivo en z es horario, es decir, a la derecha. Pasar de este marco al del coche es girar 180 grados alrededor de x: cambian de signo y y z. Una placa montada boca abajo, o un código copiado de un dron, dan un giroscopio invertido.

imu_adapter traduce de la placa al coche con tres claves de robot.yaml: forward_axis, left_axis y up_axis. Cada una dice qué eje de la placa apunta hacia adelante, hacia la izquierda y hacia arriba, con su signo. Por ejemplo, left_axis: '-x' significa “el eje x de la placa apunta a la derecha del coche”. El adaptador exige que los tres formen un marco de mano derecha (adelante por izquierda tiene que dar arriba); si no, se cae al arrancar con is not a right-handed frame [sim]. Hay 24 combinaciones válidas.

El montaje plano del 8 oct era forward_axis: '+y', left_axis: '-x', up_axis: '+z' [coche]. Con la RRC Lite y la Raspberry de canto (montaje del 21 oct) esos valores ya no valen: por eso robot.yaml dice measured: false y imu_adapter lo avisa en cada arranque.

Elige cómo está montada la placa en el coche y qué le dices a imu_adapter. La tabla muestra qué lee tu estrategia al girar el coche a mano. El caso inicial es el clásico: la placa está de canto, del “otro lado” de lo que dice la configuración.

Encuentra los ejes: dos métodos

Método 1, el del equipo: imu_axes. Con el coche quieto sobre la mesa y solo el driver de la placa corriendo (detén el resto del launch):

ros2 run robotek_hardware imu_axes

Espera 3 s sin tocar el coche: la gravedad da el eje de arriba. Cuando diga PUSH, da un solo empujón suave hacia adelante y suelta: la primera ráfaga de aceleración da el eje de adelante, y el de la izquierda sale de esos dos. Imprime el bloque imu_adapter: listo para pegar en robot.yaml y error_deg, lo lejos que está cada dirección medida del eje de la placa más cercano. Si algún error_deg pasa de unos 15 grados, la placa está inclinada o el empujón fue torcido: repite [código].

Trampa: si empujas hacia atrás sin querer, adelante e izquierda salen invertidos. Sigue siendo un marco válido, así que nadie se queja; el giroscopio z queda bien, pero el acelerómetro x queda al revés [código].

Método 2, tres giros a mano, solo con el giroscopio. No hace falta empujar el coche. Mira los ejes de la placa con ros2 topic echo --field angular_velocity /rrc/imu_raw y fíjate en qué eje responde y con qué signo:

1. Gira el coche a la izquierda sobre la mesa (antihorario visto desde arriba). Responde el eje que apunta arriba. Si el eje a de la placa sale positivo, up_axis: '+a'; si sale negativo, up_axis: '-a'.
2. Baja el morro (levanta la cola). Es un giro positivo alrededor del eje de la izquierda. Si responde el eje b en positivo, left_axis: '+b'; en negativo, '-b'.
3. Inclina el coche a la derecha (rueda derecha abajo). Es un giro positivo alrededor del eje de adelante. Si responde el eje c en positivo, forward_axis: '+c'; en negativo, '-c'.

Comprueba que adelante por izquierda da arriba. Si no sale, repetiste un giro con el signo cambiado. El acelerómetro confirma: en reposo marca +9,8 en el eje de arriba, y si levantas el morro marca positivo en el de adelante [código].

Comprobación final (con measured: true ya puesto). Corre imu_probe.py, que lee /robotek/imu, ya en el marco del coche. Tres giros a mano:

Si z da -90, el signo está invertido: up_axis tiene el signo contrario (normalmente también left_axis, porque dar la vuelta a la placa alrededor del eje de avance cambia el signo de izquierda y de arriba a la vez). Si z da unos 5000, el driver publica grados por segundo (caso 11). Si z casi no se mueve, up_axis apunta a un eje horizontal.

Arréglalo. Pega el bloque en src/robotek_hardware/config/robot.yaml, bajo imu_adapter, y pon measured: true. Relanza. Desde ahí /robotek/imu ya viene en el marco del coche y el resto del sistema no cambia.

Y el reglamento. Prohíbe hacer calibraciones de sensores en el coche durante la preparación de una ronda (§9.9). Determina los ejes en la práctica y déjalos fijos. El sesgo del giroscopio lo mide tu estrategia después del Start.

Comprueba que quedó. imu_probe.py pasa las tres pruebas. Después, con el coche en el suelo y a velocidad baja, ya no se acumula el desvío en las rectas y la primera esquina termina en 1 o 2 s.

Caso 5: se encabrita, derrapa o vuelca

Cómo se ve. Al arrancar levanta las ruedas delanteras. O en una esquina derrapa de cola y se va contra el muro exterior. O acaba de lado o boca arriba.

Causas, de más a menos probable.

1. Arranque a fondo. Desde parado a 100 por ciento el coche se encabrita: las ruedas delanteras quedan en el aire y la dirección no actúa hasta que vuelven a tocar el suelo. Con 50 por ciento desde parado el morro empieza a levantarse (la aceleración vertical cae a 4,4 m/s2 en vez de 8) [coche].
2. Más rápido que el modelo. La tabla de velocidades se midió con la batería baja (7,0 a 7,4 V). Con la batería cargada, el mismo porcentaje da más velocidad [coche]. Con 90 por ciento y 7,6 V, el coche iba “mucho más rápido que antes” y chocó (intento 7). Y por encima del 30 por ciento la recta de calibración no vale: el coche iba un 25 a 30 por ciento más lento de lo pedido con la batería casi descargada [coche]. La velocidad depende de la carga, del porcentaje y del suelo.
3. Frenar de golpe. El primer freno del driver era invertir el motor 0,12 s: las ruedas derrapaban y la deceleración era brutal [coche]. Hoy el driver frena con una rampa (stop_ramp_s).
4. Velocidad alta con dirección a tope. La aceleración lateral en una curva es v^2 * tan(ángulo) / distancia_entre_ejes. Con la dirección a 17,6 grados y la distancia de 0,1174 m: a 0,6 m/s son unos 1,0 m/s2 (0,1 g); a 1,2 m/s son 3,9 (0,4 g); a 1,5 m/s, 6,1 (0,6 g) [código].
5. Enganchar algo. El borde del tapete, una pared, una rueda atascada.

ros2 topic echo /motor_vel            # el porcentaje que llega al motor (nunca más que motor_max_pct)
ros2 topic echo /cmd_vel_raw          # la velocidad que pide tu estrategia
ros2 topic echo --field linear_acceleration.z /robotek/imu   # en el arranque, bajar de unos 5 m/s2 = morro arriba

ros2 topic pub -r 20 -t 60 /motor_vel std_msgs/msg/Float32 "{data: 30.0}"

Caso 6: no gira lo suficiente, o gira demasiado

Cómo se ve. No gira lo suficiente: en la esquina abre la trayectoria y roza o choca con el muro exterior; el radio de giro es mayor de lo que esperabas. Gira demasiado: se pasa de la esquina, mete el morro en la isla o zigzaguea.

El ángulo de rueda pasa por cuatro límites en cadena. El primero que corta es el que manda:

1. Tu estrategia: STEER_LIMIT (0,30718 rad, 17,6 grados) en minisim.py y REAL_STEER_LIMIT en robotek_common/vehicle.py.
2. base_adapter: wheel_limit_deg (17.6) recorta el ángulo de rueda que pide la estrategia, simétrico a los dos lados.
3. La tabla wheel_table_* convierte ese ángulo de rueda en ángulo de servo, y steer_limit_deg (25) recorta el del servo.
4. El servo y la mecánica: servo_min_us/servo_max_us (1000 a 2050 µs) y el tope físico de la dirección.

Causas, de más a menos probable.

1. La tabla de ruedas no es la del coche actual (montaje o distancia entre ejes cambiados). Las ruedas giran menos de lo que cree el modelo.
2. Una rueda gira menos que la otra. En el montaje anterior, con 25 grados de servo la rueda daba unos 19,7 a la izquierda y 17,6 a la derecha, o sea, ~2 grados de diferencia [coche]. wheel_limit_deg existe para igualar los dos lados.
3. Subviraje a velocidad. Con más velocidad, las ruedas delanteras patinan y el coche abre la curva.
4. El servo no llega a la posición por ser lento (caso 7): el comando cambia más rápido que la rueda.
5. El servo choca con un tope y el recorte servo_min_us/servo_max_us no es el del montaje.

Caso 7: el servo vibra, zumba, se calienta o va lento

Cómo se ve. El servo zumba sin parar aunque el coche vaya recto, la dirección tiembla, el servo se calienta, o en las esquinas el coche se pasa de largo y corrige tarde.

Causas, de más a menos probable.

1. Empuja contra un tope. Si servo_min_us/servo_max_us (o steer_limit_deg) piden más recorrido del que tiene la mecánica, el servo se queda empujando y zumba y se calienta [código]. En las pruebas de banco del equipo, las órdenes de -25 grados (1069 µs) quedaban cerca del tope de 1000 µs [coche].
2. Comandos que oscilan. base_adapter manda una orden al servo por cada mensaje de velocidad (50 por segundo desde drive.py), redondeada a µs. Si la dirección que pide tu estrategia tiembla un poco por el ruido del LiDAR, el servo recibe 50 órdenes por segundo ligeramente distintas y zumba. servo_steer (la herramienta de banco) solo reenvía cuando el valor cambia; base_adapter no [código].
3. Tensión equivocada. El servo va conectado al puerto 2 de la placa (conector J4). Según la documentación de Hiwonder, la placa tiene un puente para alimentar los servos a 5 V o a la tensión de entrada: comprueba a qué está antes de cambiar de servo [código].
4. Holgura o roce mecánico en la palanca o las varillas.
5. Es más lento de lo que supone el modelo. El coche del equipo tiene un retardo de unos 60 ms entre la orden y el primer movimiento (52 a 79 ms) y tarda unos 0,15 a 0,2 s en recorrer 10 grados y 0,4 a 0,5 s en 25 grados [coche]. El simulador usa retardo 0,06 s, constante de tiempo 0,05 s y 1,2 rad/s (SERVO_DELAY, SERVO_TAU, SERVO_RATE en minisim.py).

import time

import rclpy
from rclpy.node import Node
from rrc_lite_msgs.msg import RGBState, RGBStates, SetAckerServoState


def servo(position):
    msg = SetAckerServoState()
    msg.position = position
    msg.duration = 0.05
    return msg


def led(on):
    state = RGBState()
    state.index, state.red, state.green, state.blue = 1, 255 if on else 0, 0, 0
    msg = RGBStates()
    msg.states.append(state)
    return msg


rclpy.init()
node = Node('servo_step')
servo_pub = node.create_publisher(SetAckerServoState, '/rrc/acker_servo/set_state', 10)
rgb_pub = node.create_publisher(RGBStates, '/rrc/set_rgb', 10)
time.sleep(1.0)                          # descubrimiento
servo_pub.publish(servo(1500))
time.sleep(1.5)
rgb_pub.publish(led(True))               # el LED y el servo, en el mismo instante
servo_pub.publish(servo(1879))           # +20 grados con el montaje antiguo; usa tu valor
time.sleep(1.5)
servo_pub.publish(servo(1500))
rgb_pub.publish(led(False))
time.sleep(0.5)
rclpy.shutdown()

Caso 8: el motor

Cómo se ve. No se mueve, se mueve a tirones, gira al revés o va a una velocidad que no es la pedida.

ros2 topic hz /cmd_vel_raw     # unos 50 Hz; si llega a menos de 3 por segundo, el perro guardián (0.3 s) lo para
ros2 topic echo /motor_vel     # ¿llega algún porcentaje distinto de 0?

Caso 9: LiDAR

Cómo se ve. Los LED se ponen en rojo con azul (falta el LiDAR). O hay datos pero el coche “ve” la pared por detrás. O el mapa sale espejado. O hay muchos huecos y la estrategia duda.

Lo normal en el coche del equipo (LD19) [coche]:

Causas, de más a menos probable.

1. No hay datos. El puerto serie no está, otro proceso lo tiene abierto (dos lectores en el mismo puerto dan timeout [coche]), o el LiDAR arrancó a la vez que la placa y falló el arranque: el equipo lanza el LiDAR unos 5 s después de la placa (car.launch.py de este curso no los separa). Después no se reprodujo, así que puede haber sido otra cosa, como dos lectores en el puerto [coche]. Si moviste un USB con el stack en marcha, hay que relanzar: los procesos antiguos se quedan sin datos [coche].
2. angle_offset equivocado. El valor correcto para el LD19 con el cable atrás es π (angle_offset: 3.14159...). Con 0, el “frente” de tu estrategia es la parte trasera del LiDAR [código].
3. reverse equivocado. Si el driver da los ángulos en sentido horario, hay que poner reverse: true. El LD19 del equipo no lo necesita [coche].
4. Zona ciega. fov_deg: 110 y min_valid_range: 0.12 descartan lo que ve el propio coche. En un montaje nuevo, la zona ciega cambia: vuelve a medirla. En el coche del equipo se hizo con 30 vueltas por grado [coche].
5. Huecos de más: superficies muy oscuras o brillantes, luz solar directa o el tapete. Con el plano de escaneo a 30 mm del suelo, un pliegue o una junta del tapete de más de 3 cm aparece como una pared falsa [código]. El LD19 no se ha probado con sol directo en las pruebas del equipo.

ls -l /dev/serial/by-id/                    # el LiDAR debe aparecer
ros2 topic hz /scan                         # unos 10 Hz, tal como sale del driver
ros2 topic hz /robotek/scan                 # lo mismo, ya con 0 rad al frente
ros2 topic echo --field header.frame_id --once /robotek/scan

Caso 10: cámara y sentido de la vuelta

Cómo se ve. Sin imagen. O colores cambiados. O el coche decide el sentido contrario y choca en la primera esquina. O tarda muchos segundos en decidir. O se queda en rojo fijo (DIRECTION_FAULT).

En tu estrategia la cámara solo decide una cosa: el sentido de la vuelta (DirectionVote: línea azul más larga = antihorario, naranja = horario). Pero es la decisión más cara si falla.

Causas, de más a menos probable.

1. Un falso color en el fondo. Es lo que le pasó al equipo (intento 1): la pared beige de la sala, por encima del muro negro del campo, cayó dentro del rango naranja (en una imagen, 434 píxeles de naranja y 0 de azul), así que el coche decidió “horario” con la esquina abierta antihoraria [coche]. Los rangos HSV del curso (BLUE_HSV, ORANGE_HSV en vision.py) son los del simulador.
2. Rojo y azul intercambiados. Si el nodo de cámara declara bgr8 pero los datos son rgb8, azul y naranja se cambian y el sentido sale al revés [código]. Mira la codificación: ros2 topic echo --field encoding --once /usb_cam/image_raw.
3. Línea demasiado corta o no visible al arrancar. En el intento 6, la línea naranja se veía corta (74 a 322 px) y el sentido tardó 12,4 s [coche]. El tiempo máximo en tu estrategia es DIRECTION_TIMEOUT = 15 s: pasado ese tiempo, STOP con DIRECTION_FAULT y los LED en rojo fijo.
4. Pocas imágenes por segundo. La cámara del equipo da entre 6 y 9 imágenes por segundo con exposición automática, y 4,6 con huecos de 4,8 s si la CPU está saturada [coche]. DirectionVote necesita 5 imágenes seguidas que coincidan (CONFIRMATIONS = 5), o sea, al menos 0,8 s a 6 fps [código], y tu drive.py descarta el voto si pasan 0,5 s sin imagen. La estrategia del equipo, con 3 confirmaciones, tardaba entre 0,9 y 1,1 s en calibrar y decidir [coche].
5. Iluminación. La exposición automática alarga el obturador con poca luz y baja los fps. Con exposición manual se llegó a 17 fps, pero la imagen sale más oscura y hay que recomprobar los colores [coche].
6. Inclinación y giro de la cámara. Mira demasiada sala por encima del muro o la línea cae fuera de la imagen. En el coche del equipo, medida el 8 oct con el montaje anterior, la cámara mira unos 10 grados hacia abajo, con campo de visión de unos 67 grados y girada 4,4 grados a la izquierda (REAL_CAMERA_* en vehicle.py) [coche]. Con el montaje nuevo (cámara más alta y más atrás) hay que volver a medir.
7. Sin imagen: la cámara se enchufó después de arrancar (el nodo muere), la ruta /dev/videoN cambió, o el USB se cayó. En una prueba de 90 s, el kernel registró USB disconnect y la cámara volvió como otro /dev/videoN, y el nodo no se recuperaba solo [coche].

ros2 topic hz /usb_cam/image_raw                       # unos 5 a 15 Hz
ros2 topic echo --field encoding --once /usb_cam/image_raw   # bgr8 o rgb8
ls -l /dev/v4l/by-id/                                  # la ruta estable de la cámara

import cv2
import rclpy
from cv_bridge import CvBridge
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image

from robotek_course.vision import line_lengths


class Grab(Node):
    def __init__(self):
        super().__init__('grab_frame')
        self.bridge = CvBridge()
        self.done = False
        self.create_subscription(Image, '/usb_cam/image_raw', self.on_image, qos_profile_sensor_data)

    def on_image(self, m):
        if self.done:
            return
        bgr = self.bridge.imgmsg_to_cv2(m, desired_encoding='bgr8')
        cv2.imwrite('frame.png', bgr)
        hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
        h, w = bgr.shape[:2]
        print('azul y naranja (px):', line_lengths(bgr), '| HSV del centro:', hsv[h // 2, w // 2])
        self.done = True


rclpy.init()
node = Grab()
while rclpy.ok() and not node.done:
    rclpy.spin_once(node, timeout_sec=0.5)
rclpy.shutdown()

Caso 11: giroscopio, sesgo, ruido y unidades

Cómo se ve. El coche con el giroscopio bien orientado (caso 4) se desvía despacio, o el rumbo “se va” aunque esté quieto, o los valores no tienen el tamaño de un giro real.

Lo que midió el equipo [coche] (placa plana, el 8 y 9 oct):

El sesgo depende del eje: en la placa de canto, el eje que ahora es z del coche era otro en el montaje plano. Un sesgo de 0,047 rad/s son 2,7 grados por segundo; si ese eje acabara como tu up_axis, la calibración lo quitaría igual, pero verifica [código].

Causas, de más a menos probable.

1. Sesgo residual. Tu estrategia mide el sesgo en 0,3 s (15 muestras a 50 Hz). Eso deja un error de unos 0,0005 rad/s por ruido, pero el sesgo cambia con la temperatura y la medida de 0,3 s no lo sigue. En una ronda real el rumbo de la pared pasó de +0,03 a -0,14 rad en unos 35 s: un sesgo residual de unos 0,005 rad/s [coche]. En 42 s eso son 12 grados.
2. Calibrar con el coche en movimiento (caso 2), o un control que no va a 50 Hz: drive.py integra con dt = 1/50 fijo (caso 17).
3. Placa recién encendida. El MPU6050 se calienta al encender y el sesgo se mueve en los primeros minutos [código].
4. Unidades. Medidas del equipo: aceleración de 9,62 m/s2 y ruido de 0,002 rad/s, o sea, que el driver ya da unidades ROS (m/s2 y rad/s) [coche]. El protocolo de la placa habla en g y grados por segundo, y el driver convierte. Si cambias de driver, comprueba con imu_probe.py: un giro de 90 grados debe dar ~90. Si da ~5200, el driver da grados por segundo; si da ~1,6, convirtió dos veces.
5. Ruido por vibración. La placa de canto vibra al rodar [código]. Compara la desviación con el coche quieto y con la rueda motriz girando en el aire.
6. Saturación. Un giro a tope a 0,6 m/s son unos 1,6 rad/s (unos 93 grados por segundo); cualquier rango del MPU6050 de ±250 grados por segundo lo cubre. Un golpe contra un muro puede saturarlo, pero un golpe ya es un fallo [código].

Caso 12: reinicios y USB que se caen

Cómo se ve. En plena ronda el coche se queda parado de golpe. Se pierde la conexión por SSH. Los LED se apagan y se encienden. Falta algún sensor. O el Pi se reinicia y los datos de la ronda se pierden.

Lo que se encontró en el coche del equipo [coche]:

• La tira de LED al 100 por ciento apagó el Pi. Treinta LED blancos al máximo toman unos 1,8 A del raíl de 5 V del Pi. Se quitó la tira; si la usas, aliméntala con un regulador aparte.
• Un cable flojo (el del motor, sobre todo) causó tres de los ocho intentos. Siempre que algo “no hace nada”, tira de los conectores primero.
• El Pi se reinició varias veces más con la tira apagada, también al copiar archivos, sin diagnóstico claro. Después de reiniciar, vcgencmd get_throttled vuelve a 0x0 y ya no sirve para saber qué pasó.
• Con el motor a 80 por ciento y la batería en 6,5 V, el Pi se reinició a los 73 s (intento 5). En el intento 7, con 90 por ciento, el operador paró el coche cortando la alimentación, y los datos de la ronda, que estaban en un búfer, se perdieron.
• No era la potencia: en las pruebas de motor, el raíl de 5 V del Pi no bajó de 4,8 V (el umbral de subtensión es de unos 4,63 V) y la batería se mantuvo por encima de 7 V. Los reinicios coincidieron con la tira LED y con cables flojos.
• La cámara se desconectó sola una vez a los 90 s (USB disconnect).
• Un USB movido de puerto hace que el LiDAR y la cámara cambien de nombre: relanza el stack. Usa las rutas /dev/serial/by-id/... y /dev/v4l/by-id/....
• La placa se resetea y pierde el puerto serie. El driver del equipo reabre el puerto si el USB se cae; el de este curso no: relanza el launch [coche].
• Vibración. Los conectores de una placa de canto trabajan al rodar: sujétalos [código].

vcgencmd get_throttled                          # 0x0 = bien. Bit 0: subtensión ahora. Bit 16: hubo subtensión desde el arranque
vcgencmd measure_temp                           # temperatura
watch -n 0.5 "vcgencmd pmic_read_adc EXT5V_V"   # la tensión de 5 V de la Raspberry Pi 5; debajo de ~4,63 V hay subtensión
sudo dmesg -T | grep -i -E "usb|under-voltage|disconnect"   # qué USB se cayó
journalctl -b -1 -n 50 --no-pager               # el final del arranque anterior, si se reinició

Caso 13: paradas y final de la ronda

Cómo se ve. Se para antes de acabar. No se para. Se pasa de la sección de salida. Para antes de llegar.

Lo que hace tu estrategia al final (mission.py): tras la esquina 12 entra en FINISH, avanza a FINISH_SPEED = 0.15 hasta que la pared de enfrente está a FINISH_LEAD = 0.08 m de la distancia que midió en el Start (_start_front), frena (se enclava) y a los STOP_WAIT = 0.9 s pasa a DONE.

Se para antes de acabar

1. Stop del operador. Una segunda pulsación del botón, aunque sea sin querer (antirrebote de 1,5 s). Relanza para correr otra vez.
2. DIRECTION_FAULT. Sin sentido en 15 s (caso 10): LED rojo fijo y el coche ni arranca.
3. Se atascó (STALLED). Solo la estrategia del equipo lo detecta. Pasó con el cable del motor suelto [coche]. La tuya, si no avanza, se queda esperando.
4. Se cayó el nodo (caso 17).
5. Frenó por sensor viejo (el freno del equipo, caso 2).

No se para

• FINISH solo termina si la pared de enfrente es finita. Si front_axis es inf (LiDAR ciego, caso 9), el coche no avanza ni termina en FINISH.
• Si el motor no mueve el coche con 0,15 m/s (con la calibración, 17,5 por ciento), el coche se queda parado en FINISH sin llegar. Con la batería baja puede pasar [código].

Se pasa de la sección de salida o para antes

El coche para donde la pared de enfrente vuelve a estar a la distancia del Start. Si en el Start lo pusiste al fondo de la sección, para al fondo; si lo pusiste al borde delantero, un poco de inercia lo saca. Y la rampa de parada del motor (0,6 s) añade unos centímetros.

En el intento 8, el FINISH duró solo 0,9 s: la distancia al frente al terminar la última esquina (1,47 m) ya era menor que la del Start (1,61 m) más el margen, así que frenó en seguida, y al llegar a DONE los dos lados estaban abiertos, o sea que pudo parar antes de la sección de salida. No se verificó [coche].

Diagnostica. Después de DONE, mide a mano dónde quedó el coche con respecto a la sección de salida, y anota en cada ronda front_axis en el Start y al parar (scan_probe.py o tu propio log). Si el coche se mueve después de DONE, mira el eje del giroscopio: el giroscopio tras DONE no vale si alguien levantó el coche (en el intento 2 salió +2,6 rad en 60 s tras DONE con la desviación subiendo a 0,27 rad/s: era manipulación) [coche].

Arréglalo.

Comprueba que quedó. Diez rondas, y en todas el coche queda con todo el cuerpo dentro de la sección de salida. Registra el resultado en la tabla de rondas; una ronda “buena” no vale si no mides dónde paró.

Caso 14: roza la isla o el muro exterior en las esquinas

Cómo se ve. Después de girar, el morro toca el muro de la isla. O en la última esquina el lado del coche roza el muro exterior. Rozar la isla sin moverla no penaliza, y tocar el muro exterior en el Open no está permitido (§9.18 del reglamento).

En el intento 8 el operador vio dos roces a la isla, justo después de las esquinas 4 y 11 (el LiDAR marcaba el frente a 0,15 a 0,21 m). Además, en la última esquina el LiDAR marcó el frente a 0,12 m y el muro exterior a 0,13 m: un posible roce que nadie confirmó [coche].

La esquina tiene una puerta con cuatro condiciones a la vez (turn_gate.py): armada, avanzado ARM_DISTANCE, pared de enfrente a menos de TURN_FRONT y fin de la isla visto (LOOKAHEAD, o isla abierta a más de ISLAND_OPEN). Cada número mueve el punto donde empieza el giro.

Causas, de más a menos probable.

1. Velocidad mayor que la del modelo (caso 5): el coche llega a la esquina más tarde de lo que el modelo supone y se abre.
2. Umbrales afinados para otro campo. TURN_FRONT = 1,19 sirve para el campo square100; el Open cambia de tamaño y de pasillos (de 0,5 a 1,1 m).
3. Cambió la posición del LiDAR. Los umbrales están en el marco del LiDAR. En el coche del equipo, el montaje nuevo dejó el LiDAR 68 mm más adelante que cuando se afinaron los umbrales, y el fin de la isla se ve unos 6,8 cm antes [código].
4. El LiDAR falla en el fin de la isla. Huecos al azar (4,3 por ciento) pueden romper la línea de la pared: MAX_GAP_BEAMS = 2 y CONFIRMATIONS = 2 protegen, pero con muchos más huecos (caso 9), el coche cree que la isla acabó y gira demasiado pronto.
5. Giroscopio con sesgo (caso 11): el objetivo goal se va desviando del muro real.
6. Dirección con menos giro del que cree el modelo (caso 6).

        if self.gate.update(scene, inner, self.travelled, self.arc):
            print(f'esquina {self.completed + 1}: frente={scene.front_axis:.2f} fin_isla={scene.island_end:.2f} '
                  f'isla={inner:.2f} v={corner_speed}')

Caso 15: funciona en el simulador y no en el coche

Esta tabla resume las diferencias que más fallos causaron. La última columna dice dónde está el detalle.

Una forma de acercar el simulador al coche es cambiar en minisim.py las constantes (SPEED_TABLE, SERVO_*, GYRO_BIAS, SCAN_DROPOUT, STEER_LIMIT) por tus medidas. El paso 12 trata cómo afinar con ellas.

Caso 16: red, latencia y ROS_DOMAIN_ID

Cómo se ve. Desde tu portátil no ves los nodos del coche. RViz no muestra la cámara. Ves los nodos pero con datos viejos. Dos coches se pisan.

Lo que falló al equipo [coche]: el portátil no veía ningún nodo del Pi y RViz no mostraba la cámara. Eran cuatro causas a la vez: (1) el dominio era distinto (42 frente a 1); (2) el descubrimiento estaba en LOCALHOST en un lado y en SUBNET en el otro; (3) un lado usaba Fast DDS y el otro Cyclone, que no se vieron; (4) el demonio de ROS guardaba la configuración anterior (ros2 daemon stop). El síntoma: ros2 node list en el portátil solo lista nodos del portátil.

echo $ROS_DOMAIN_ID $RMW_IMPLEMENTATION $ROS_AUTOMATIC_DISCOVERY_RANGE   # los tres, iguales en las dos máquinas
ros2 daemon stop && ros2 node list
ros2 node list                                       # ¿hay nodos repetidos?

Caso 17: un nodo se cae

Cómo se ve. El launch imprime process has died, o un traceback, y el coche se queda sin hacer algo (sin dirección, sin motor, sin estrategia).

Caso 18: aviso de batería baja

Cómo se ve. Los dos LED parpadean en rojo. El coche sigue funcionando.

battery_guard lee /rrc/battery (en milivoltios, una vez por segundo) y, tras 5 lecturas seguidas por debajo de 6,8 V, publica /robotek/battery_low y status_leds parpadea en rojo. No para el coche (decisión del equipo, 9 oct) y el aviso queda hasta relanzar [coche].

• 6,8 V son 3,4 V por celda de la LiPo 2S: queda poca carga. Por debajo de unos 3,0 V por celda (6,0 V) la batería se daña, y 6 V es también el mínimo de la placa.
• Mide la batería del motor aparte. Lo que mide la placa es la de la electrónica. Si el motor tiene su propia batería, nadie la vigila: usa un medidor.
• Una lectura suelta no cuenta. Las lecturas de 0 mV (fallos de la placa) se ignoran.
• La batería del equipo se midió entre 7,0 y 8,1 V; en reposo 7,26 V. Tras unas horas de pruebas llegó a 6,40 V.
• Contrasta una vez con un multímetro. Según la documentación de Hiwonder, la placa v1.0 tiene dos patas del chip puenteadas a propósito para medir la batería; sin ese puente la lectura saldría mal. La lectura de 7,26 V es plausible, pero battery_guard depende de ella.

La batería importa para algo más que el aviso: la misma ronda, con un motor al 40 por ciento y 6,66 V, tardó 47 s; con 50 por ciento y 7,40 V, 40 s [coche]. Anota el voltaje de cada ronda.

Checklist antes de cada ronda

Imprímela. Marca cada punto, uno por uno. Los marcados con (*) se pueden hacer en el pasillo en dos minutos.

1. Batería de la electrónica cargada, ros2 topic echo /rrc/battery --once por encima de 7000 mV; batería del motor aparte, medida (caso 18). Anota los dos valores.
2. Cables y conectores (motor, servo, LiDAR, cámara, placa): tira de cada uno. Ninguno a menos de 10 mm de una rueda o de la palanca del servo (casos 3, 12).
3. (*) Un solo ros2 launch vivo: ros2 node list sin repetidos (caso 16).
4. (*) LED en verde, mirados de cerca. Rojo con un color dice qué sensor falta: azul LiDAR, magenta IMU, amarillo cámara (casos 9, 4, 10).
5. (*) Frecuencias con ros2 topic hz: LiDAR unos 10 Hz, IMU unos 50 Hz, cámara más de 5 Hz y /cmd_vel_raw a 50 Hz.
6. (*) Giroscopio: quieto 10 s, media de z entre -0,01 y +0,01; un giro a mano a la izquierda da z positivo (caso 4).
7. (*) Servo, con las ruedas en el aire: +steer va a la izquierda, 0 queda recto (caso 3).
8. (*) LiDAR: mano delante, ángulo 0; mano a la izquierda, ángulo +90 (caso 9).
9. Cámara: una imagen guardada con el coche en el Start; los colores se ven bien y la línea mide más de 40 px (caso 10).
10. motor_max_pct fijo y anotado (casos 5, 8).
11. Coche en la sección de salida. Pulsa el botón con un dedo y aparta la mano de inmediato: no sostengas el coche ni lo muevas durante el primer segundo, que es cuando calibra (caso 2). Ten la otra mano lista para el Stop.
12. No cambies calibraciones ni datos en la preparación de la ronda (§9.9). Los ejes de la IMU, el centro del servo y los colores se miden en la práctica.
13. Tras la ronda: anota batería, tope del motor, resultado, dónde paró y qué roces hubo; guarda el log (tee).

Diagnóstico en cinco minutos

Cuando algo falla y no sabes por dónde empezar, haz esto en orden y para en el primer paso que no dé lo esperado.

Qué anotar cuando algo falla

Un fallo bien anotado vale más que diez rondas buenas. Para cada intento, también los fallidos, guarda:

• Fecha, hora, batería (electrónica y motor), motor_max_pct, versión del código.
• Qué estado tenía /robotek/state al fallar, y qué pedía /cmd_vel_raw.
• Lo que viste con tus ojos: si giró bien, dónde chocó, de qué lado.
• El log del launch (tee) y, si puedes, un vídeo del móvil.
• La causa probable, marcada como hipótesis hasta que la compruebes.

Con ese registro el equipo pudo separar, entre ocho intentos, tres cables sueltos, un sentido mal decidido, una velocidad por encima del modelo y un reinicio sin explicar. Si te encuentras algo que no está aquí, apúntalo con el mismo formato.