micro-ROS发布JointState时rosidl_runtime_c__double__Sequence类型及赋值报错问题

问题原因 #

1. micro-ROS的C语言接口中，sensor_msgs/msg/JointState的name、position等可变长字段均为rosidl_runtime_c__XX__Sequence类型，这是micro-ROS实现的动态数组结构，包含size（当前元素个数）、capacity（内存可容纳最大元素个数）、data（数据存储指针）三个成员，使用前需要手动分配内存，你代码中未分配内存直接访问data属于非法操作。
2. name序列的单个元素是rosidl_runtime_c__String结构体类型，不能直接赋值C语言字符串常量，必须使用官方提供的字符串赋值函数处理内存拷贝。
3. 原代码将rcl_publish放在数据赋值之前，会导致首次发布的消息为空。

解决方法 #

步骤1：定义关节数量 #

在代码开头宏定义部分，添加你需要发布的关节数量：

// 示例为4个车轮关节，可根据实际情况修改
#define JOINT_NUM 4

步骤2：初始化消息内存 #

在setup()函数的rclc_support_init执行完成后，添加消息内存分配和关节名初始化代码：

// 给关节名称序列分配内存
msg.name.data = (rosidl_runtime_c__String*)allocator.allocate(sizeof(rosidl_runtime_c__String) * JOINT_NUM, allocator.state);
msg.name.capacity = JOINT_NUM;
msg.name.size = JOINT_NUM;

// 给位置序列分配内存
msg.position.data = (double*)allocator.allocate(sizeof(double) * JOINT_NUM, allocator.state);
msg.position.capacity = JOINT_NUM;
msg.position.size = JOINT_NUM;

// 不需要速度、力矩字段直接将size设为0即可
msg.velocity.size = 0;
msg.effort.size = 0;

// 初始化关节名称，使用专用字符串赋值函数
rosidl_runtime_c__String__assign(&msg.name.data[0], "drivewhl_1g_joint");
rosidl_runtime_c__String__assign(&msg.name.data[1], "drivewhl_1d_joint");
rosidl_runtime_c__String__assign(&msg.name.data[2], "drivewhl_2g_joint");
rosidl_runtime_c__String__assign(&msg.name.data[3], "drivewhl_2d_joint");

步骤3：修改定时器回调逻辑 #

调整回调函数的执行顺序，先更新位置数据再发布消息：

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{  
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    // 更新位置数据，此处替换为实际读取的编码器数值
    msg.position.data[0] = 1.85;
    msg.position.data[1] = 0.2;
    msg.position.data[2] = 0;
    msg.position.data[3] = 0;

    // 发布消息
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
  }
}

完整可运行代码 #

#include <micro_ros_arduino.h>

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

#include <sensor_msgs/msg/joint_state.h>

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

#define LED_PIN 13
// 自定义关节数量
#define JOINT_NUM 4

#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);
  }
}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{  
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    // 替换为实际编码器读取逻辑
    msg.position.data[0] = 1.85;
    msg.position.data[1] = 0.2;
    msg.position.data[2] = 0;
    msg.position.data[3] = 0;

    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
  }
}

void setup() {
  set_microros_transports();
  
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);  
  
  delay(2000);

  allocator = rcl_get_default_allocator();

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

  // 初始化JointState消息内存
  // 给关节名称序列分配内存
  msg.name.data = (rosidl_runtime_c__String*)allocator.allocate(sizeof(rosidl_runtime_c__String) * JOINT_NUM, allocator.state);
  msg.name.capacity = JOINT_NUM;
  msg.name.size = JOINT_NUM;

  // 给位置序列分配内存
  msg.position.data = (double*)allocator.allocate(sizeof(double) * JOINT_NUM, allocator.state);
  msg.position.capacity = JOINT_NUM;
  msg.position.size = JOINT_NUM;

  // 不需要速度、力矩字段直接将size设为0即可
  msg.velocity.size = 0;
  msg.effort.size = 0;

  // 初始化关节名称
  rosidl_runtime_c__String__assign(&msg.name.data[0], "drivewhl_1g_joint");
  rosidl_runtime_c__String__assign(&msg.name.data[1], "drivewhl_1d_joint");
  rosidl_runtime_c__String__assign(&msg.name.data[2], "drivewhl_2g_joint");
  rosidl_runtime_c__String__assign(&msg.name.data[3], "drivewhl_2d_joint");

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

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),
    "joint_states"));

  // create timer,
  const unsigned int timer_timeout = 1000;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

}

void loop() {
  delay(100);
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}

内容的提问来源于stack exchange，提问作者Guillaume Sion