# Conversión del proyecto de ROS 1 a ROS 2

### Análisis inicial del proyecto rbcar\_steering\_controller (enfoque hardware)

A alto nivel, este paquete ROS controla el giro (steering) de un vehículo (Rbcar) usando:

- Un variador/drive de motor (AMC DZCANTE-020L080) controlado por CANopen para mover el actuador de dirección en modo posición.
- Un encoder absoluto EPC (familia MA) conectado también por CANopen para medir el ángulo real de la dirección.
- Una tarjeta CAN PEAK (PCAN) como interfaz física con el bus CAN.

A continuación desgloso los bloques de hardware y cómo se integran:

#### 1) Bus CAN y hardware de comunicaciones

- Interfaz: PEAK PCAN (usuarios típicamente PCAN-PCI, PCAN-USB, etc.). En el launch se usa el dispositivo “/dev/pcanpci1”; por defecto en código “/dev/pcan32”.
- Velocidad: 1 Mbit/s (CAN\_BAUD\_1M).
- Librerías: libpcan (PCan.h/PCan.cc) para abrir y configurar el canal, y enviar/recibir tramas CAN.
- Protocolo: CANopen “ligero” implementado manualmente, sin stack externo. Se envían NMT (Start, Stop, Reset Comm/Node), SDO (objetos de configuración/lectura) y PDOs (tramas de proceso).
- Mecanismo de supervisión: Node Guard/Heartbeat (cíclicamente se lanza un RTR a 0x700+NodeID y se comprueba el estado; si no hay respuesta dentro de NMT\_TIMEOUT=2s se considera fallo de comunicación).

#### 2) Drive de motor: AMC DZCANTE-020L080

- Función: accionar el actuador de dirección en modo posición (Position Mode).
- Nodo CAN: ID configurable (por defecto can\_motor\_id=1).
- Clase base **Dzcante020l080**.\* encapsula objetos CAN (índices SDO/PDO) para este drive: 
    - ControlWord (0x6040) con comandos: ENABLE\_OP, SHUTDOWN, SWITCH\_ON, QUICK\_STOP, DISABLE\_VOLT, RESET.
    - Modos de operación (0x6060) = Position/Velocity/Home.
    - RPDO y TPDO: configuración de COB-IDs y modos de transmisión (objetos 0x140x/0x180x y similares).
    - Lecturas: StatusWord (0x6041), Drive Status extendido (0x2002 subíndices 1..6), posición (0x6064), velocidad (0x60FF), corriente (0x6077), tensión bus DC (0x200F.01).
- Clase **MotorDrive**.\* añade: 
    - State machine interna (INIT/READY/EMERGENCY...) y comunicación (OPERATIONAL/FAULT...).
    - Hilos auxiliares: 
        - ControlCommunicationThread: envía Node Guard y vigila tiempos para marcar FAULT si no hay respuesta.
        - ReadSDOMessagesThread: lectura cíclica de Drive Status, tensión del bus, entradas digitales, posición actual, etc., y cálculo de medias.
    - Configuración CAN: 
        - Reset/Preoperational, setting de Node Guard (Guard Time 0x100C, Life Factor 0x100D), Event Action (0x2065.21), Recovery Time, TPDOs/RPDOs para posición/velocidad.
    - Gestión de estados de variador: dsDesiredStatus vs dsStatus. Cambia de estados enviando ControlWord con tiempos de guarda.
    - Señales de diagnóstico: descompone StatusWord y 0x2002.\* en flags con máscaras y temporizadores para logging-control.
- Clase **MotorPosition**.\* especializa a Position Mode: 
    - Inicialización: secuencia ControlWord (Disable Voltage, Shutdown, Switch On, Enable Operation) y lectura de modo operación.
    - Homing: soportado (START\_HOME / STOP\_HOME), flags de homing completo y cambio a modo posición.
    - SetMotorPosition: envía objetivo vía RPDO21 (POSITION\_PDO\_MSG) en cuentas (counts).

#### 3) Encoder absoluto EPC (familia MA) por CANopen

- Función: medir posición de la dirección en cuentas absolutas.
- Nodo CAN: por defecto can\_encoder\_id=0x7F.
- Clase **EpcMaCanOpen**.\* maneja NMT/SDO/PDO básicos para estos encoders (IDs TPDO1 0x180+ID, TPDO2 0x280+ID).
- Clase **EpcEncoder**.\*: 
    - State machine similar (OPERATIONAL/FAULT...).
    - Hilo de comunicación para Node Guard.
    - Procesa TPDO1/TPDO2 con cuentas del encoder (encoder\_counts).
- El nodo de alto nivel convierte estas cuentas a ángulo mediante una calibración lineal.

#### 4) Nodo ROS de control de dirección

- Binario: rbcar\_steering\_controller.cpp.
- Flujos: 
    - Parametrización vía ROS params y YAML (config/config.yaml): rangos de encoder, zero, resolución, calibración lineal (y=Ax+B), límites angulares y puntos extremos.
    - Inicializa PCan (a 1M), crea objetos MotorPosition (drive) y EpcEncoder (encoder) con sus IDs y desired\_freq (50 Hz).
    - start(): Setup y Start de ambos (abre CAN, setea, pone OPERATIONAL, lanza hilos).
    - Loop spin(): a 50 Hz: 
        - Envío SYNC (0x80).
        - Lectura de hasta 100 mensajes CAN y dispatch a Motor/Encoder para proceso (PDO/SDO/NMT/EMCY).
        - Publicación del estado: SteeringControllerStatus (estado motor+encoder, cuentas abs y motor, posición angular).
    - Suscripciones: 
        - command\_position\_counts (std\_msgs/Int32): manda counts al motor (posición absoluta en cuentas del motor).
        - command\_angle (std\_msgs/Float64): convierte ángulo deseado a cuentas absolutas y luego a cuentas de motor según relación abs\_to\_motor, y envía referencia de posición al drive.
    - Servicio: 
        - set\_motor\_status (SetMotorStatus.srv): permite fijar estado del drive: OPERATION\_ENABLED, QUICK\_STOP, READY\_TO\_SWITCH\_ON.
    - Diagnósticos: 
        - diagnostic\_updater reporta estado de comunicación y estado del drive, y supervisa frecuencia de comandos.

Qué tener en cuenta para replicar:

- **Calibración mecánica**: el YAML aporta relación entre encoder absoluto y cuentas del motor, y los extremos físicos. Esto es clave para que el comando en rad -> counts funcione correctamente.
- La variable abs\_to\_motor se calcula como motor\_counts\_range / abs\_counts\_range. Debes medir ambos rangos en tu mecánica y actualizar config.yaml.
- value\_calibrate\_a y value\_calibrate\_b definen la recta ángulo = -a\*counts + b. Ajustarlos a tu encoder y cinemática.
- Límites de ángulo: extrem\_angle\_left/right y sus points asociados (extrem\_points\_left/right) fijan saturaciones de seguridad.

#### 5) Resumen de la secuencia de arranque/operación

1. PCan abre el dispositivo y configura el bitrate.
2. MotorDrive: 
    - Reset CAN Comm -> Preoperational
    - Configura PDOs/Node Guard/Eventos
    - Init drive (ControlWord sequence), lee modo operación
    - Start Communication (NMT Start), pasa a OPERATIONAL
    - Lanza hilos de Node Guard y lecturas SDO periódicas
3. EpcEncoder: 
    - Reset/Preoperational
    - (En este código no reconfigura PDOs del encoder)
    - Start Communication a OPERATIONAL
    - Lanza hilo de Node Guard
4. Bucle 50 Hz: 
    - Envía SYNC
    - Lee paquetes CAN y los distribuye
    - Publica estado (counts + angle)
5. Comandos: 
    - command\_angle -> se traduce a counts del encoder y luego a counts del motor y se envía la posición al drive por RPDO21.

#### 6) Señales CAN-clave y mapeo rápido

- NMT (ID 0x000): Start 0x01, Stop 0x02, Reset Comm 0x82, Reset Node 0x81, Preop 0x80 (DATA\[0\]=cmd, DATA\[1\]=node).
- Node Guard RTR a 0x700+NodeID, respuesta en 0x700+NodeID con estado (OPERATIONAL=0x05/0x85, PREOP=0x7F/0xFF, STOPPED=0x04/0x84).
- SYNC: 0x80 sin datos.
- Drive: 
    - SDO server 0x600+ID (Write/Read a objetos 0x6040 ControlWord, 0x6060 Mode, 0x607A Position, 0x60FF Velocity, 0x2002 Drive Status, 0x200F.01 DC Bus, 0x6064 Position actual).
    - RPDO21 (0x280+ID) para consigna de posición (4 bytes little-endian).
    - TPDO1/21/22 para StatusWord/Position/Velocity según configuración.
- Encoder EPC: 
    - TPDO1 (0x180+ID) / TPDO2 (0x280+ID): se usan los 2 bytes bajos como cuentas (el código así lo interpreta).
    - Node Guard/Heartbeat igual que el drive.

#### 7) Qué adaptar en tu nuevo proyecto (hardware)

- Si cambias: 
    - Interfaz CAN: sustituye PCan.\* por tu driver y conserva la API Send/Read/Setup/Configure.
    - Drive: si el drive no es AMC DZCANTE-020L080, necesitarás: 
        - Revisar/actualizar índices SDO, COB-IDs, TPDO/RPDO, escalados, y status words.
        - Ajustar secuencias de ControlWord y modos de operación.
    - Encoder absoluto: si otro fabricante/protocolo, cambiar mapeo de TPDOs y cómo se extraen cuentas.
    - Mecánica: recalibrar config.yaml para rangos, cero y linealización ángulo–cuentas.
- Mantener: 
    - Supervisión de comunicación (Node Guard/Heartbeat) o migrar a Heartbeat puro si tus nodos no usan Node Guard clásico.
    - Seguridad: QUICK\_STOP y manejo de fallos del drive.

[![photo_2025-10-13_08-33-26.jpg](https://wiki.racarla.es/uploads/images/gallery/2025-10/scaled-1680-/photo-2025-10-13-08-33-26.jpg)](https://wiki.racarla.es/uploads/images/gallery/2025-10/photo-2025-10-13-08-33-26.jpg)