# Análisis software del sistema de tracción con ROS 1 [https://github.com/racarla96/ros1\_caddy\_ai2\_rbcar\_robot/tree/indigo-devel/curtis\_motordrive](https://github.com/racarla96/ros1_caddy_ai2_rbcar_robot/tree/indigo-devel/curtis_motordrive) A continuación se presenta un análisis general del código contenido en el archivo **curtis\_controller.cpp** del paquete *curtis\_controller*, que forma parte del sistema de control para un motor drive basado en ROS: --- ## 1. Arquitectura General y Objetivos - **Propósito:** El controlador (*CurtisController*) se encarga de gestionar la comunicación entre la red CAN y el sistema ROS para controlar un motor (o conjunto de motores) de la plataforma. Esto incluye la recepción de comandos (ya sea de tipo “throttle” o “twist”), la lectura de datos desde el bus CAN y la publicación del estado del sistema. - **Componentes Clave:** - **Interfaz CAN:** Se utiliza un objeto `PCan` (representado por `can_dev`) para la comunicación con el bus CAN. - **Control del Motor:** El objeto `MasterDrive` se encarga de enviar comandos de aceleración (throttle) y de procesar los mensajes CAN que provienen del drive. - **Integración ROS:** - **Publicadores:** Publica el estado general en el topic `"state"` y datos específicos del drive en `"master_drive"` y `"slave_drive"`. - **Suscriptores:** Se suscribe a comandos de control (por ejemplo, `"command_throttle"` o `"command_twist"`) dependiendo del modo configurado. - **Servicios:** Posee un servicio para resetear el controlador. --- ## 2. Máquina de Estados El funcionamiento central del controlador se organiza en una máquina de estados implementada en el método `controlLoop()`, la cual revisa el valor de la variable `state` y ejecuta la función correspondiente. Los estados son definidos mediante constantes (probablemente en el paquete `robotnik_msgs::State`): - **INIT\_STATE:** - **Función:** `initState()` - **Acción:** Llama a `setup()` para inicializar los subcomponentes (CAN, drive, etc.). - **Transición:** Si la inicialización es exitosa, cambia al estado **READY\_STATE**. - **STANDBY\_STATE:** - **Función:** `standbyState()` - **Acción:** Se enfoca en la lectura de mensajes del bus CAN (llamando a `readCANMessages()`), pero sin enviar comandos de throttle. - **READY\_STATE:** - **Función:** `readyState()` - **Acción:** - Lee mensajes CAN y, en base a ellos, gestiona la comunicación con el drive. - Verifica condiciones de seguridad, como la existencia de la interlock. - Envía el valor de aceleración (throttle) a través de `master_drive->sendThrottle()`, aplicando límites para evitar exceder el rango permitido. - **Control de Fallos:** Si falla la lectura de mensajes CAN o se detecta un error al enviar el throttle, se transita a **FAILURE\_STATE**. - **FAILURE\_STATE:** - **Función:** `failureState()` - **Acción:** Permite una recuperación tras un tiempo predefinido (controlado por `failure_recover_time`). Se intenta reinicializar el sistema, y si tiene éxito, se cambia a **READY\_STATE**. - **SHUTDOWN\_STATE:** - **Función:** `shutdownState()` - **Acción:** Ejecuta el proceso de apagado (`shutdown()`) de los dispositivos y transita de vuelta al estado **INIT\_STATE**. - **EMERGENCY\_STATE:** - **Función:** `emergencyState()` - **Acción:** Actualmente se define pero sin implementación. Se reserva para tratar situaciones de emergencia. --- ## 3. Flujo de Ejecución y Control - **Inicio y Bucle de Control:** El método `start()` configura ROS (llamando a `rosSetup()`), activa la bandera `running` y ejecuta el `controlLoop()`, el cual se ejecuta a una frecuencia determinada (`desired_freq_`) leída desde el servidor de parámetros o con un valor por defecto. - **Control Loop:** Dentro de este ciclo: - Se registra el tiempo para calcular la frecuencia real de ejecución. - Se ejecuta el bloque correspondiente al estado actual (con `switch(state)`). - Se llaman a funciones comunes en todas las iteraciones a través de `allState()`, que entre otras cosas, implementa un watchdog (para poner a cero el throttle si no se reciben comandos en un tiempo determinado) y publica el estado a ROS. - **Detección y Procesamiento de Mensajes CAN:** El método `readCANMessages()` intenta leer varios mensajes (en un bucle) del bus CAN y, para cada mensaje leído, llama a `master_drive->processCANMessage()`. Esto es fundamental para actualizar el estado del drive (por ejemplo, datos de velocidad, fault codes, interlock, etc.). --- ## 4. Interacción mediante ROS - **Suscriptores:** Dependiendo del modo configurado (definido en el parámetro `"mode"`), se suscribe a: - `"command_throttle"`: Para recibir comandos de aceleración directa. - `"command_twist"`: Para recibir comandos en forma de twist (probablemente para convertir velocidad lineal y angular a un comando de throttle). - **Publicadores:** - **Estado del Controlador:** Publica un mensaje del tipo `robotnik_msgs::State` que incluye el estado actual, la frecuencia deseada y la real, así como una descripción en cadena del estado. - **Datos del Motor:** Publica datos específicos de la conducción (por ejemplo, velocidad, interlock, fallos) en dos topics: `"master_drive"` y `"slave_drive"` con mensajes del tipo `curtis_msgs::DriveData`. - **Servicio de Reset:** Se implementa un servicio denominado `"reset"` que permite reiniciar el controlador (a través de la función `resetSrv`). --- ## 5. Gestión de Errores y Seguridad - **Watchdog:** En el método `allState()`, se monitoriza el tiempo desde el último comando recibido. Si se supera un tiempo límite (`WATCHDOG_TIMEOUT`), se fuerza la reducción del throttle a cero para evitar movimientos inesperados. - **Control de Interlock:** El sistema verifica constantemente el estado del interlock: - Si no está activado, se establece el throttle deseado a cero para prevenir el funcionamiento sin la debida seguridad. - Se detecta la transición en el interlock (por ejemplo, cuando se recupera) y se emiten mensajes de advertencia. - **Transiciones por Error:** Si se producen errores en la lectura de mensajes CAN o en el envío de comandos a través del drive, el sistema transita al estado **FAILURE\_STATE** y posteriormente intenta una recuperación. --- ## Conclusión El código del *CurtisController* está organizado de forma modular y robusta, utilizando una máquina de estados que permite gestionar de manera clara las diferentes fases de la operación: - **Inicialización y Configuración (INIT\_STATE y rosSetup):** Se leen parámetros, se configuran los dispositivos CAN y se preparan los publishers/subscribers de ROS. - **Operación Normal (READY\_STATE):** Se realizan lecturas y escrituras constantes sobre el bus CAN, se envían comandos al drive y se publica el estado. - **Gestión de Fallos (FAILURE\_STATE y EMERGENCY\_STATE):** Se implementa una estrategia para detectar fallos y recuperar el control tras un tiempo de espera. - **Apagado Seguro (SHUTDOWN\_STATE):** Se asegura la liberación de recursos y se prepara el sistema para una nueva inicialización. En resumen, se trata de un controlador para sistemas embebidos en robótica que integra comunicación a bajo nivel (CAN) con la capa de mensajería y servicios de ROS, siguiendo un patrón basado en estados que facilita tanto la operación normal como el manejo de situaciones de error o emergencia.