Início / coding / Perguntas / 79592649
Accepted
Giuseppe Festa
Asked: 2025-04-25 21:42:07 +0800 CST

Problemas de assinante no ESP32 executando Micro-ros

  • 772

Estou trabalhando em um projeto micro-ROS usando o ESP32 conectado a um PC via serial (USB) a 921.600 bauds. Embora a funcionalidade de publicador funcione bem, quando adiciono um assinante, o ESP32 entra em um loop de erros constantemente no momento em que inicio o agente micro-ROS.

Este é o arquivo de inicialização e comando atual, onde outro nó de teste é executado para verificar a publicação e a assinatura

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',
        ),
    ])

Configurar

  • Microcontrolador ESP32 (ambiente esp32 rebaixado para 2.0.2 para resolver problemas de compatibilidade com microros)
  • Agente micro-ROS em execução no PC (Ubuntu 22.04)
  • Nó Python ROS2 ( angle_listener.py) que assina dados do codificador e publica comandos de velocidade
  • Conexão serial via USB (UART0) a 921600 baud
  • A configuração é apenas um teste, então algumas das soluções (como as matrizes unidimensionais de tamanho 1 por causa de N_MOTORS) são para escalar o código até 6 motores posteriormente

Limitação de hardware

Minhas capacidades de depuração são limitadas porque a porta serial de hardware que estou usando para comunicação USB (UART0) é a única disponível na placa que estou usando. Isso significa que não posso adicionar instruções Serial.print para depuração.

Código

Trecho de código ESP32 (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;
}

Nó Python:

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

Comportamento

  1. O ESP32 funciona corretamente apenas com o publicador (dados do codificador)
  2. Quando adiciono o código do assinante, o ESP32 entra imediatamente na função de loop de erro
  3. O loop de erro é acionado pela falha de uma das macros RCCHECK

O que eu tentei

  • Os nomes dos tópicos verificados correspondem entre o ESP32 e o nó Python
  • Verifiquei se os tópicos foram inicializados corretamente
  • Aumento do tempo de rotação no esp32 para garantir que o problema não esteja relacionado à CPU

O que pode estar fazendo com que o ESP32 entre no loop de erro ao adicionar um assinante? Qualquer ajuda seria muito apreciada!

python

1 respostas

  1. Best Answer

    Aparentemente, o problema estava relacionado a um erro na documentação que eu estava usando. Em particular, RCCHECK(std_msgs__msg__Float64__init(&sub_msg))a linha que estava gerando o erro.

    Meu código estava chamando a macro RCCHECK em uma função que retorna um bool, não um rcl_ret_t. Desde então true != RCL_RET_OK (which is 0), ele aciona instantaneamente o manipulador de erros. Esse erro também se aplica a todos os outros inicializadores do publicador de tipos.

    • 0

relate perguntas