🔌 micro-ROS en el ESP32

# descubre el puerto del ESP32 (DevKit: ttyUSB0; S3 con USB nativo: ttyACM0)
ls /dev/ttyUSB* /dev/ttyACM*
# agente por el puerto serial (cable USB)
docker run -it --rm -v /dev:/dev --privileged --net=host \
  microros/micro-ros-agent:<distro> serial --dev /dev/ttyUSB0 -b 115200
# O agente por Wi-Fi (UDP), para el ESP32 sin cable
docker run -it --rm --net=host microros/micro-ros-agent:<distro> udp4 --port 8888

// Base del robot con micro-ROS (biblioteca micro_ros_arduino de TU distro, paquete esp32 3.x)
// Publica /ticks (encoders) cada 50 ms y se suscribe a /cmd_vel (geometry_msgs/Twist).
#include <micro_ros_arduino.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32_multi_array.h>
#include <geometry_msgs/msg/twist.h>

const int PINO_LED = 2;
const int AIN1 = 26, AIN2 = 27, PWMA = 25;   // motor izquierdo (TB6612FNG)
const int BIN1 = 16, BIN2 = 17, PWMB = 4;    // motor derecho
const int STBY = 23;
const int ENC_ESQ = 18, ENC_DIR = 19;       // canal A de cada encoder
const float SEPARACAO = 0.15;              // distancia entre ruedas, en m: MÍDELA en tu robot
const float V_MAX = 0.40;                  // m/s con PWM 255: mídela con una cinta métrica
const unsigned long TIMEOUT_MS = 500;      // sin comando durante 0,5 s = se detiene

volatile int32_t ticksEsq = 0, ticksDir = 0;
volatile int sentidoEsq = 1, sentidoDir = 1;
unsigned long ultimoCmd = 0;

rcl_publisher_t pubTicks;
rcl_subscription_t subCmd;
std_msgs__msg__Int32MultiArray msgTicks;
int32_t bufTicks[2];
geometry_msgs__msg__Twist msgCmd;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

#define RCCHECK(fn) { rcl_ret_t rc = fn; if (rc != RCL_RET_OK) erro(); }
#define RCSOFT(fn)  { rcl_ret_t rc = fn; (void)rc; }

void erro() {                               // LED parpadeando rápido = error al crear algo
  ledcWrite(PWMA, 0); ledcWrite(PWMB, 0);
  while (true) { digitalWrite(PINO_LED, !digitalRead(PINO_LED)); delay(100); }
}

void IRAM_ATTR isrEsq() { ticksEsq += sentidoEsq; }
void IRAM_ATTR isrDir() { ticksDir += sentidoDir; }

// v en m/s (negativo = marcha atrás) se convierte en dirección + PWM de 0 a 255
void motor(int in1, int in2, int pwm, float v, volatile int &sentido) {
  int duty = constrain((int)(fabs(v) / V_MAX * 255.0), 0, 255);
  digitalWrite(in1, v >= 0 ? HIGH : LOW);
  digitalWrite(in2, v >= 0 ? LOW : HIGH);
  ledcWrite(pwm, duty);
  sentido = (v >= 0) ? 1 : -1;               // encoder de un canal: la dirección viene del comando
}

// Tópico 4: llega un Twist en /cmd_vel
void aoReceberCmd(const void *msgin) {
  const geometry_msgs__msg__Twist *cmd = (const geometry_msgs__msg__Twist *)msgin;
  float v = cmd->linear.x, w = cmd->angular.z;
  motor(AIN1, AIN2, PWMA, v - w * SEPARACAO / 2.0, sentidoEsq);
  motor(BIN1, BIN2, PWMB, v + w * SEPARACAO / 2.0, sentidoDir);
  ultimoCmd = millis();
}

// Tópico 3: cada 50 ms publica los ticks y comprueba el watchdog
void aoTimer(rcl_timer_t *t, int64_t ultimo) {
  (void)ultimo; if (t == NULL) return;
  bufTicks[0] = ticksEsq; bufTicks[1] = ticksDir;
  RCSOFT(rcl_publish(&pubTicks, &msgTicks, NULL));
  if (millis() - ultimoCmd > TIMEOUT_MS) { ledcWrite(PWMA, 0); ledcWrite(PWMB, 0); }
}

void setup() {
  pinMode(PINO_LED, OUTPUT);
  set_microros_transports();                // serial USB a 115200 (¡no uses Serial.print!)
  int saidas[] = {AIN1, AIN2, BIN1, BIN2, STBY};
  for (int p : saidas) pinMode(p, OUTPUT);
  digitalWrite(STBY, HIGH);                 // saca el controlador del modo de espera
  ledcAttach(PWMA, 20000, 8);               // 20 kHz, 8 bits (API 3.x)
  ledcAttach(PWMB, 20000, 8);
  pinMode(ENC_ESQ, INPUT_PULLUP); pinMode(ENC_DIR, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC_ESQ), isrEsq, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_DIR), isrDir, RISING);
  delay(2000);                              // tiempo para que el agente atienda

  allocator = rcl_get_default_allocator();
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  RCCHECK(rclc_node_init_default(&node, "esp32_base", "", &support));
  RCCHECK(rclc_publisher_init_default(&pubTicks, &node,
          ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32MultiArray), "ticks"));
  RCCHECK(rclc_subscription_init_default(&subCmd, &node,
          ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "cmd_vel"));
  // Jazzy y Lyrical usan la versión "2" del temporizador (distros anteriores: rclc_timer_init_default)
  RCCHECK(rclc_timer_init_default2(&timer, &support, RCL_MS_TO_NS(50), aoTimer, true));
  RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));
  RCCHECK(rclc_executor_add_subscription(&executor, &subCmd, &msgCmd, &aoReceberCmd, ON_NEW_DATA));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  msgTicks.data.data = bufTicks;            // el mensaje apunta al vector fijo de 2
  msgTicks.data.size = 2;
  msgTicks.data.capacity = 2;
  digitalWrite(PINO_LED, HIGH);             // LED encendido fijo = conectado
}

void loop() {
  RCSOFT(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)));
}

# 1) ambas ruedas hacia adelante a 0,15 m/s durante 3 segundos (10 mensajes/s)
timeout 3 ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.15}}"
# 2) girar sobre su eje: ruedas en sentidos opuestos
timeout 3 ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/Twist "{angular: {z: 1.0}}"
# 3) conducir con el teclado, igual que en Gazebo en 4.2
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# Convierte /ticks (ESP32) en /odom y en la TF odom -> base_link
import math
import rclpy
from rclpy.node import Node
from rclpy.executors import ExternalShutdownException
from std_msgs.msg import Int32MultiArray
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster

class Odometria(Node):
    def __init__(self):
        super().__init__('odometria')
        self.tpv = self.declare_parameter('ticks_por_volta', 360).value
        self.raio = self.declare_parameter('raio', 0.033).value        # m
        self.sep = self.declare_parameter('separacao', 0.15).value     # m, igual que en el firmware
        self.x = self.y = self.th = 0.0
        self.anterior, self.t_anterior = None, None
        self.pub = self.create_publisher(Odometry, 'odom', 10)
        self.tf = TransformBroadcaster(self)
        self.create_subscription(Int32MultiArray, 'ticks', self.ao_receber, 10)

    def ao_receber(self, msg):
        if len(msg.data) < 2:
            return
        agora, ticks = self.get_clock().now(), list(msg.data[:2])
        if self.anterior is None:                  # primer mensaje: solo guarda
            self.anterior, self.t_anterior = ticks, agora
            return
        m_por_tick = 2 * math.pi * self.raio / self.tpv
        d_esq = (ticks[0] - self.anterior[0]) * m_por_tick
        d_dir = (ticks[1] - self.anterior[1]) * m_por_tick
        dt = (agora - self.t_anterior).nanoseconds / 1e9
        self.anterior, self.t_anterior = ticks, agora
        d = (d_esq + d_dir) / 2                      # cuánto avanzó el centro
        dth = (d_dir - d_esq) / self.sep             # cuánto giró
        self.x += d * math.cos(self.th + dth / 2)
        self.y += d * math.sin(self.th + dth / 2)
        self.th += dth
        qz, qw = math.sin(self.th / 2), math.cos(self.th / 2)   # ángulo -> quaternion
        stamp = agora.to_msg()

        odom = Odometry()
        odom.header.stamp, odom.header.frame_id, odom.child_frame_id = stamp, 'odom', 'base_link'
        odom.pose.pose.position.x, odom.pose.pose.position.y = self.x, self.y
        odom.pose.pose.orientation.z, odom.pose.pose.orientation.w = qz, qw
        if dt > 0:
            odom.twist.twist.linear.x, odom.twist.twist.angular.z = d / dt, dth / dt
        self.pub.publish(odom)

        t = TransformStamped()
        t.header.stamp, t.header.frame_id, t.child_frame_id = stamp, 'odom', 'base_link'
        t.transform.translation.x, t.transform.translation.y = self.x, self.y
        t.transform.rotation.z, t.transform.rotation.w = qz, qw
        self.tf.sendTransform(t)

def main(args=None):
    rclpy.init(args=args)
    no = Odometria()
    try:
        rclpy.spin(no)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass
    finally:
        no.destroy_node()
        rclpy.try_shutdown()

if __name__ == '__main__':
    main()

# 1) en setup.py de meu_robo, agrega en console_scripts:
#      'odometria = meu_robo.odometria:main',
# 2) en package.xml, agrega exec_depend para nav_msgs, geometry_msgs y tf2_ros
cd ~/ros2_ws && colcon build --symlink-install --packages-select meu_robo
source install/setup.bash
ros2 run meu_robo odometria --ros-args -p ticks_por_volta:=<pulsos> -p raio:=0.033 -p separacao:=0.15

char rede[] = "<sua_rede_wifi>";
char senha[] = "<sua_senha>";
char ip_agente[] = "<ip_do_pc_ou_pi>";
set_microros_wifi_transports(rede, senha, ip_agente, 8888);   // en lugar de set_microros_transports()