🔌 micro-ROS on the ESP32

# find the ESP32 port (DevKit: ttyUSB0; S3 with native USB: ttyACM0)
ls /dev/ttyUSB* /dev/ttyACM*
# agent over the serial port (USB cable)
docker run -it --rm -v /dev:/dev --privileged --net=host \
  microros/micro-ros-agent:<distro> serial --dev /dev/ttyUSB0 -b 115200
# OR agent over Wi-Fi (UDP), for the ESP32 without a cable
docker run -it --rm --net=host microros/micro-ros-agent:<distro> udp4 --port 8888

// Robot base with micro-ROS (micro_ros_arduino library for YOUR distro, esp32 package 3.x)
// Publishes /ticks (encoders) every 50 ms and subscribes to /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;   // left motor (TB6612FNG)
const int BIN1 = 16, BIN2 = 17, PWMB = 4;    // right motor
const int STBY = 23;
const int ENC_ESQ = 18, ENC_DIR = 19;       // channel A of each encoder
const float SEPARACAO = 0.15;              // distance between wheels in m: MEASURE your robot
const float V_MAX = 0.40;                  // m/s at PWM 255: measure with a tape measure
const unsigned long TIMEOUT_MS = 500;      // no command for 0.5 s = stop

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() {                               // rapidly blinking LED = failed to create something
  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 in m/s (negative = reverse) becomes direction + PWM from 0 to 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;               // single-channel encoder: direction comes from the command
}

// Topic 4: a Twist arrives on /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();
}

// Topic 3: publishes ticks every 50 ms and checks the 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();                // USB serial at 115200 (do not use Serial.print!)
  int saidas[] = {AIN1, AIN2, BIN1, BIN2, STBY};
  for (int p : saidas) pinMode(p, OUTPUT);
  digitalWrite(STBY, HIGH);                 // take the driver out of standby mode
  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);                              // time for the agent to respond

  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 and Lyrical use timer version "2" (older distros: 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;            // the message points to the fixed array of 2
  msgTicks.data.size = 2;
  msgTicks.data.capacity = 2;
  digitalWrite(PINO_LED, HIGH);             // solid LED = connected
}

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

# 1) both wheels forward at 0.15 m/s for 3 seconds (10 messages/s)
timeout 3 ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.15}}"
# 2) spin in place: wheels in opposite directions
timeout 3 ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/Twist "{angular: {z: 1.0}}"
# 3) drive with the keyboard, as in Gazebo 4.2
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# Converts /ticks (ESP32) into /odom and the odom -> base_link TF
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, same as the 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:                  # first message: just store it
            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                      # how far the center moved
        dth = (d_dir - d_esq) / self.sep             # how much it turned
        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)   # angle -> 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) in meu_robo's setup.py, add to console_scripts:
#      'odometria = meu_robo.odometria:main',
# 2) in package.xml, add exec_depend for nav_msgs, geometry_msgs, and 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);   // instead of set_microros_transports()