[SOLVED] 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 <Wire.h>
#include <AS5600.h>
#include <TMCStepper.h>


#include <micro_ros_arduino.h>
#include <AccelStepper.h>

#include <std_msgs/msg/float64.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <std_msgs/msg/int32.h>

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<N_MOTOR;i++){
    drivers[i].begin();
    drivers[i].toff(5);
    drivers[i].rms_current(DRIVER_CURRENT);
    drivers[i].microsteps(MICROSTEP);
    drivers[i].en_spreadCycle(false);  // StealthChop

    motors[i].setMaxSpeed(a2s(100));
  }


  set_microros_transports();

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);

  delay(100);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "ESP32_node", "", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "axis_1_encoder"));

  // create timer,
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(ENCODER_PUBLISH_TIME),
    timer_callback));

   // ### 2. initialize subscriber message
  RCCHECK(std_msgs__msg__Float64__init(&sub_msg));

  // 3. create subscriber
  RCCHECK(rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64),
    "motor_velocity_1"      // rename topic as you wish
  ));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));

  RCCHECK(rclc_executor_add_timer(&executor, &timer));

    // 4. add subscription to executor
  RCCHECK(rclc_executor_add_subscription(
    &executor,
    &subscriber,
    &sub_msg,
    &velocity_callback,
    ON_NEW_DATA
  ));

  mux.begin();
  mux.selectChannel(0);

  encoder.begin();
}


void loop() {
  // Gestione micro-ROS
  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(20));
  for (int i = 0; i < N_MOTOR; i++) {
    motors[i].runSpeed();
  }
}


float a2s(float angular_velocity_rad_s) {
  // steps_per_rev is typically 200 for 1.8°/step motors
  float rev_per_sec = angular_velocity_rad_s / (2.0f * M_PI);
  float steps_per_sec = rev_per_sec * STEPS_PER_REV * MICROSTEP;
  return steps_per_sec;
}

Python Node:

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()

Behavior

ESP32 works correctly with just the publisher (encoder data)
When I add the subscriber code, the ESP32 immediately enters the error loop function
The error loop is triggered by one of the RCCHECK macros failing

What I've Tried

Verified topic names match between ESP32 and Python node
Verified that the topics are correctly initialized
Increased spin timing on esp32 to ensure the problem wasn't related to cpu

What could be causing the ESP32 to enter the error loop when adding a subscriber Any help would be greatly appreciated!