RPi-Pico使用micro-ROS多话题发布故障排查求助

核心问题诊断 #

1. 消息数据未实时更新：仅在setup()中初始化了rpm消息的data值，后续loop()未同步编码器的实时数据，导致发布的是固定初始值。多话题场景下，无效数据累积会引发通信链路异常。
2. 通信调度阻塞：loop()中的delay(50)会阻塞micro-ROS的通信处理线程，加上spin_some超时设置过长，导致多话题消息无法及时被处理，队列堆积后引发代理端无响应或冻结。
3. 默认内存限制：RPi-Pico上的micro-ROS默认静态内存池不足以支撑4个publisher的同时创建与消息发送，部分话题的通信资源分配失败。

修复步骤 #

1. 实时更新发布消息数据 #

在loop()的通信处理前，同步编码器的最新rpm值到发布消息中：

// 在spin调用前添加
rpm_lf.data = Motor_LF.encoder.rpm;
rpm_lb.data = Motor_LB.encoder.rpm;
rpm_rf.data = Motor_RF.encoder.rpm;
rpm_rb.data = Motor_RB.encoder.rpm;

2. 优化通信调度逻辑 #

• 移除loop()中的delay(50)，避免阻塞通信线程；
• 缩短rclc_executor_spin_some的超时时间，让executor更频繁处理通信事件：

RCSOFTCHECK(rclc_executor_spin_some(&executor_pub, RCL_MS_TO_NS(10)));

3. 调整micro-ROS内存配置 #

在Arduino IDE中修改micro-ROS编译参数，增大静态内存池：

• 打开文件 > 首选项，勾选“显示详细输出”的编译选项；
• 在编译参数中添加：

  -DUROS_MEM_POOL_SIZE=5120 -DUROS_MAX_PUBLISHERS=4
  

  （内存池大小可按需调整，示例为5120字节）

4. 可选：合并话题减少内存占用 #

若内存仍紧张，可将四个编码器数据打包为自定义消息（如MotorRPMs），仅用一个publisher发布，降低内存消耗。

修改后的完整代码 #

#include <micro_ros_arduino.h>

#include "pinout.h"
#include "motor.h"
#include <math.h>

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

#include <geometry_msgs/msg/twist.h>
#include <std_msgs/msg/float32.h>
#include <std_msgs/msg/int16.h>


#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

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


// declare motors
Motor Motor_LF(motor_lf, dir_lf1, dir_lf2, true, LF_ENCODER_P, LF_ENCODER_N);
Motor Motor_LB(motor_lb, dir_lb1, dir_lb2, true, LB_ENCODER_P, LB_ENCODER_N);
Motor Motor_RF(motor_rf, dir_rf1, dir_rf2, true, RF_ENCODER_P, RF_ENCODER_N);
Motor Motor_RB(motor_rb, dir_rb1, dir_rb2, true, RB_ENCODER_P, RB_ENCODER_N);

//uROS declarations
rcl_node_t node;
rclc_support_t support;
rcl_allocator_t allocator;

//Declare uROS Pub/Sub
// Pub
rcl_publisher_t encLF_pub;
rcl_publisher_t encLB_pub;
rcl_publisher_t encRF_pub;
rcl_publisher_t encRB_pub;

std_msgs__msg__Int16 rpm_lf;
std_msgs__msg__Int16 rpm_lb;
std_msgs__msg__Int16 rpm_rf;
std_msgs__msg__Int16 rpm_rb;

rclc_executor_t executor_pub;
rcl_timer_t timer;



// Hardware interrupt handler functions

void cntEnc0() {
// cntEnc is activated if DigitalPin is going from LOW to HIGH
// Check pin to determine direction
    if(digitalRead(LF_ENCODER_N)==LOW) 
      Motor_LF.encoder.encoderCount++;
    else 
      Motor_LF.encoder.encoderCount--;
}
void cntEnc1() {
// cntEnc is activated if DigitalPin is going from LOW to HIGH
// Check pin to determine the direction
    if(digitalRead(LF_ENCODER_P)==LOW) 
      Motor_LF.encoder.encoderCount--;
    else 
      Motor_LF.encoder.encoderCount++;
}

void cntEnc2() {

    if(digitalRead(LB_ENCODER_N)==LOW) 
      Motor_LB.encoder.encoderCount++;
    else 
      Motor_LB.encoder.encoderCount--;
  
}
void cntEnc3() {

    if(digitalRead(LB_ENCODER_P)==LOW) 
      Motor_LB.encoder.encoderCount--;
    else 
      Motor_LB.encoder.encoderCount++;

}

void cntEnc4() {

    if(digitalRead(RF_ENCODER_N)==LOW) 
      Motor_RF.encoder.encoderCount++;
    else 
      Motor_RF.encoder.encoderCount--;
  
}
void cntEnc5() {

    if(digitalRead(RF_ENCODER_P)==LOW) 
      Motor_RF.encoder.encoderCount--;
    else 
      Motor_RF.encoder.encoderCount++;

}

void cntEnc6() {

    if(digitalRead(RB_ENCODER_N)==LOW) 
      Motor_RB.encoder.encoderCount++;
    else 
      Motor_RB.encoder.encoderCount--;
  
}
void cntEnc7() {
 
    if(digitalRead(RB_ENCODER_P)==LOW) 
      Motor_RB.encoder.encoderCount--;
    else 
      Motor_RB.encoder.encoderCount++;
}


// motion directions
float power_y = 0, power_x = 0, power_z = 0, alpha = 0;


void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    RCSOFTCHECK(rcl_publish(&encLF_pub, &rpm_lf, NULL));
    RCSOFTCHECK(rcl_publish(&encLB_pub, &rpm_lb, NULL));
    RCSOFTCHECK(rcl_publish(&encRF_pub, &rpm_rf, NULL));
    RCSOFTCHECK(rcl_publish(&encRB_pub, &rpm_rb, NULL));
  
  }
}


void linear_y(float power){
  Motor_RF.drive(power);
  Motor_LF.drive(power);
  Motor_RB.drive(power);
  Motor_LB.drive(power);
}


void linear_x(float power){
  Motor_RF.drive(power);
  Motor_LF.drive(-power);
  Motor_RB.drive(-power);
  Motor_LB.drive(power);
}


void angular_z(float power){ //clockwise
  Motor_RF.drive(-power);
  Motor_LF.drive(power);
  Motor_RB.drive(-power);
  Motor_LB.drive(power);
}

void mixed_motion_xy(float power_y, float power_x) {
  // 补充混合运动实现
  Motor_LF.drive(power_y + power_x);
  Motor_RF.drive(power_y - power_x);
  Motor_LB.drive(power_y + power_x);
  Motor_RB.drive(power_y - power_x);
}


// Init hardware function
void setup() {
  
  // Start serial output 
  Serial.begin(115200);
  set_microros_transports();
  delay(2000);

  allocator = rcl_get_default_allocator();
  
  // Setup hardware interrupt pins for encoder sensors
  attachInterrupt(digitalPinToInterrupt(LF_ENCODER_P), cntEnc0, RISING);   //Hardware interrupt pin 1 for encoder 
  attachInterrupt(digitalPinToInterrupt(LF_ENCODER_N), cntEnc1, RISING);   //Hardware interrupt pin 2 for encoder 

  attachInterrupt(digitalPinToInterrupt(LB_ENCODER_P), cntEnc2, RISING);
  attachInterrupt(digitalPinToInterrupt(LB_ENCODER_N), cntEnc3, RISING);

  attachInterrupt(digitalPinToInterrupt(RF_ENCODER_P), cntEnc4, RISING);
  attachInterrupt(digitalPinToInterrupt(RF_ENCODER_N), cntEnc5, RISING);

  attachInterrupt(digitalPinToInterrupt(RB_ENCODER_P), cntEnc6, RISING);
  attachInterrupt(digitalPinToInterrupt(RB_ENCODER_N), cntEnc7, RISING);


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

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

  // create publishers
  RCCHECK(rclc_publisher_init_default(
    &encLF_pub,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16),
    "rpm_lf_pub"));

  RCCHECK(rclc_publisher_init_default(
    &encLB_pub,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16),
    "rpm_lb_pub"));

  RCCHECK(rclc_publisher_init_default(
    &encRF_pub,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16),
    "rpm_rf_pub"));

  RCCHECK(rclc_publisher_init_default(
    &encRB_pub,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int16),
    "rpm_rb_pub"));

  // create timer
  const unsigned int timer_timeout = 100;
  RCCHECK(rclc_timer_init_default(
            &timer,
            &support,
            RCL_MS_TO_NS(timer_timeout),
            timer_callback));
  
  // 初始化executor，实体数量设为1（仅timer）
  RCCHECK(rclc_executor_init(&executor_pub, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor_pub, &timer));

}



// Main loop
void loop() {
  
 // Driving functions
  if(power_x == 0 && power_z == 0 ){
    linear_y(power_y);
  }
  else if(power_y == 0 && power_z == 0){
    linear_x(power_x);  
  }
  else if(power_y == 0 && power_x == 0){
    angular_z(power_z);  
  }
  else {
    mixed_motion_xy(power_y , power_x);
  }
  
  // 实时更新发布消息的最新数据
  rpm_lf.data = Motor_LF.encoder.rpm;
  rpm_lb.data = Motor_LB.encoder.rpm;
  rpm_rf.data = Motor_RF.encoder.rpm;
  rpm_rb.data = Motor_RB.encoder.rpm;
  
  // 更频繁地spin，给足通信处理时间
  RCSOFTCHECK(rclc_executor_spin_some(&executor_pub, RCL_MS_TO_NS(10)));

}

内容的提问来源于stack exchange，提问作者Mark Ataev