[Python] 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:RIVER, 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.begin();
drivers.toff(5);
drivers.rms_current(DRIVER_CURRENT);
drivers.microsteps(MICROSTEP);
drivers.en_spreadCycle(false); // StealthChop

motors.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.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

1. ESP32 works correctly with just the publisher (encoder data)
2. When I add the subscriber code, the ESP32 immediately enters the error loop function
3. 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!

Continue reading...