Subscriber issues on ESP32 running Micro-ros

I'm working on a micro-ROS project using ESP32 connected to a PC via serial (USB) at 921600 baud. While the publisher functionality works fine, when I add a subscriber , the ESP32 goes into an error loop consistently in the moment in which i launch the micro ros agent.

This is the current launch file and command, where another testing node is run to verify publishing and subscribing

ros2 launch robot_launcher robot_launcher.launch.py

from launch import LaunchDescription
from launch.actions import ExecuteProcess, SetEnvironmentVariable
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        SetEnvironmentVariable('ROS_DOMAIN_ID', '0'),
        Node(
            package='micro_ros_agent',
            executable='micro_ros_agent',
            name='micro_ros_agent',
            output='screen',
            arguments=[
                'serial',
                '--dev', '/dev/ttyACM0',
                '--baudrate', '921600',
            ],
        ),        
        Node(
            package='robot_launcher',
            executable='angle_listener',
            name='angle_listener',
            output='screen',
        ),
    ])

Setup

• ESP32 microcontroller (esp32 environment downgraded to 2.0.2 to address microros compatibility issues)
• micro-ROS agent running on PC (Ubuntu 22.04)
• Python ROS2 node (angle_listener.py) that subscribes to encoder data and publishes velocity commands
• Serial connection via USB (UART0) at 921600 baud
• The setup is just a test so some of the solutions (like the 1 dimensional arrays of size 1 because of N_MOTORS) are to scale the code up to 6 motors later

Hardware Limitation

I have limited debugging capabilities because the hardware serial port I'm using for USB communication (UART0) is the only one available on the board I'm using. This means I can't add Serial.print statements for debugging.

Code

ESP32 Code Snippet (C++):

#include 
#include 
#include 


#include 
#include 

#include 

#include 
#include 
#include 
#include 
#include 

#include 

float a2s(float angular_velocity_rad_s);

#define RCCHECK(fn)                                                    \
  {                                                                    \
    rcl_ret_t rc = fn;                                                 \
    if (rc != RCL_RET_OK) {                                            \
      Serial.print(F("ERROR at "));                                    \
      Serial.print(F(#fn));                                            \
      Serial.print(F(": "));                                           \
      Serial.println(rcl_get_error_string().str);                      \
      error_loop();                                                    \
    }                                                                  \
  }
#define RCSOFTCHECK(fn) \
  { \
    rcl_ret_t temp_rc = fn; \
    if ((temp_rc != RCL_RET_OK)) {} \
  }

#define MICROSTEP 64
#define STEPS_PER_REV 200.0
#define LED_PIN 14
// Pin definitions
#define CLK_PIN 18
#define PDN_UART_PIN 16
#define SPREAD_PIN 17

#define DIR1_PIN 4
#define STEP1_PIN 13

#define DIR2_PIN 32
#define STEP2_PIN 25


#define N_MOTOR 1

#define SERIAL_PORT Serial2
#define UART_RX_PIN 16        // PDN_UART used as UART RX
#define UART_TX_PIN 17        // SPREAD used as UART TX (optional)
#define DRIVER_ADDRESS1 0b00  // Driver address (if multiple drivers on UART)
#define DRIVER_ADDRESS2 0b01

#define R_SENSE 0.11f
#define DRIVER_CURRENT 1000  // mA, adjust based on your motor



rcl_subscription_t subscriber;
std_msgs__msg__Float64 sub_msg;
float motor_velocities[N_MOTOR] = {0.0f};

AccelStepper motors[N_MOTOR] = {
  AccelStepper(AccelStepper::DRIVER, STEP1_PIN, DIR1_PIN),
  // … fino a 6
};


TMC2209Stepper drivers[N_MOTOR]={
  TMC2209Stepper(&SERIAL_PORT, R_SENSE, DRIVER_ADDRESS1)
};


void error_loop() {
  while (1) {
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  }
}


rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

#define MULTIPLEXER_ADDR 0x70

#define ENCODER_PUBLISH_TIME 20  //ms
TwoWire &i2cBus = Wire;

// Classe per multiplexer PCA9548A
class PCA9548A {
public:
  PCA9548A(uint8_t address, TwoWire &wire = Wire)
    : _addr(address), _wire(wire) {}

  void begin() {
    _wire.begin();
  }

  void selectChannel(uint8_t channel) {
    if (channel > 7) return;
    _wire.beginTransmission(_addr);
    _wire.write(1 << channel);
    _wire.endTransmission();
  }

private:
  uint8_t _addr;
  TwoWire &_wire;
};

PCA9548A mux(MULTIPLEXER_ADDR, Wire);
AS5600 encoder(&i2cBus);



void timer_callback(rcl_timer_t *timer, int64_t last_call_time) {
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    int32_t angle = encoder.readAngle();
    msg.data = angle;
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
  }
}
void velocity_callback(const void * msgin) {
  // Cast the incoming pointer to a Float64 message
  const auto *incoming = (const std_msgs__msg__Float64 *)msgin;
  float vel = incoming->data;                     // get the single float

  motor_velocities[0] = vel;                      // assuming N_MOTOR == 1
  motors[0].setSpeed(a2s(motor_velocities[0]));   // update stepper speed
}
void setup() {
  Serial.begin(921600);
  SERIAL_PORT.begin(115200, SERIAL_8N1, UART_RX_PIN, UART_TX_PIN);
  delay(500);

  for(int i=0; i

import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32, Float64  # use Float64 instead of Float64MultiArray

class Listener(Node):

    def __init__(self):
        super().__init__("angle_listener")

        # publisher for a single float64
        self.velocity_publisher = self.create_publisher(
            Float64, 
            "motor_velocity_1", 
            10
        )

        # subscriber as before
        self.subscriber = self.create_subscription(
            Int32, 
            "axis_1_encoder", 
            self.angle_callback, 
            10
        )
        self.get_logger().info("Subscribed to encoder topic")

    def angle_callback(self, message):
        # log incoming value
        self.get_logger().info(f"Received encoder: {message.data}")

        # build and publish single Float64 with value 2.0
        vel_msg = Float64()
        vel_msg.data = 2.0
        self.velocity_publisher.publish(vel_msg)
        self.get_logger().info("Published velocity: 2.0")

def main(args=None):
    rclpy.init(args=args)
    node = Listener()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()