esp32是否支持std_msgs__msg__Float64MultiArray

harebert

在上位机（Ubuntu 22 + humble）发布了一个节点，发布消息，消息的内容是手柄上两个摇杆的值，组成的一个array：

[INFO] [1712502446.850122187] [joy_publisher_node]: Publishing Joy data to joyaxisTopic: array('d', [-0.0, -0.0])

在下位机（fishbot四驱）上编程准备接受这两个数据，并通过这两个数值来控制电机



#include <Arduino.h>

#include <micro_ros_platformio.h>

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

#include <std_msgs/msg/float64.h>
#include <Esp32McpwmMotor.h>
#include <std_msgs/msg/float64_multi_array.h>


Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
// 声明话题订阅者
rcl_subscription_t subscriber;
// 声明消息文件
// std_msgs__msg__Float64 sub_msg;
std_msgs__msg__Float64MultiArray sub_msg;

void callback_subscription_(const void *msgin)
{
  const std_msgs__msg__Float64MultiArray *msg = (const std_msgs__msg__Float64MultiArray *)msgin;
  if (msg->data.size >= 2) {
  motor.updateMotorSpeed(0, int16_t(msg->data.data[0]*100)); // 设置电机0的速度(占空比)为负70%
    motor.updateMotorSpeed(1, int16_t(msg->data.data[0]*100)); // 设置电机1的速度(占空比)为正70%
  }

}

int motorStatus = 0; // 电机状态变量，0-3循环变化

void setup()
{
  Serial.begin(115200);    // 初始化串口通信
  motor.attachMotor(0, 16, 17); // 将电机0连接到引脚23和引脚22
    motor.attachMotor(1, 12, 22); // 将电机1连接到引脚12和引脚13


  // 设置通过串口进行MicroROS通信
  set_microros_serial_transports(Serial);
  // 延时时一段时间，等待设置完成
  // delay(2000);
  // 初始化内存分配器
  allocator = rcl_get_default_allocator();
  // 创建初始化选项
  rclc_support_init(&support, 0, NULL, &allocator);
  // 创建节点 topic_sub_test
  rclc_node_init_default(&node, "topic_sub_test", "", &support);
  // 订阅者初始化
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray),
      "joyaxisTopic_0");
  // 创建执行器
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  // 为执行器添加一个订阅者
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &callback_subscription_, ON_NEW_DATA);
}

void loop()
{
    // delay(100);
  // 循环处理数据

 

     
  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
}

编译上传，代码不报错，但是启动agent以后，手柄动，数值动，电机没有任何相应。

小鱼

@harebert 要回答，我就尽量把这个东西给讲明白，在micro-ROS里，对于复杂的数据结构（包含指针的），需要提前进行内存的分配，这样当订阅到数据的时候才能把数据放入内存，然后进行回调。

接着我们来看你用到的这个std_msgs__msg__Float64MultiArray的定义。

路径: .pio/libdeps/featheresp32/micro_ros_platformio/libmicroros/include/std_msgs/msg/detail/float64_multi_array__struct.h

typedef struct std_msgs__msg__Float64MultiArray
{
  /// Please look at the MultiArrayLayout message definition for
  /// documentation on all multiarrays.
  /// specification of data layout
  std_msgs__msg__MultiArrayLayout layout;
  /// array of data
  rosidl_runtime_c__double__Sequence data;
} std_msgs__msg__Float64MultiArray;

可以看到数据部分的定义是， rosidl_runtime_c__double__Sequence data;
再来看这个的定义。

.pio/libdeps/featheresp32/micro_ros_platformio/libmicroros/include/rosidl_runtime_c/primitives_sequence.h

#define ROSIDL_RUNTIME_C__PRIMITIVE_SEQUENCE(STRUCT_NAME, TYPE_NAME) \
  typedef struct rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence \
  { \
    TYPE_NAME * data; /*!< The pointer to an array of STRUCT_NAME */ \
    size_t size; /*!< The number of valid items in data */ \
    size_t capacity; /*!< The number of allocated items in data */ \
  } rosidl_runtime_c__ ## STRUCT_NAME ## __Sequence;

这是一个用于定义的宏定义，其中data部分可以看到，是 TYPE_NAME 类型的指针，这里你可以把 TYPE_NAME 当成 double。

所以 std_msgs__msg__Float64MultiArray sub_msg; 中只是定义了一个数据的指针，数据指向的地址还是空的，所以你可以创建一个double类型的数组，然后将数组的地址传递给这个指针，当有数据的时候，就可以把数据放到这个数组中了。

double array[4];
std_msgs__msg__Float64MultiArray sub_msg;

void callback_subscription_(const void *msgin)
{
  const std_msgs__msg__Float64MultiArray *msg = (const std_msgs__msg__Float64MultiArray *)msgin;
  if (msg->data.size >= 2) {
  motor.updateMotorSpeed(0, int16_t(msg->data.data[0]*100)); // 设置电机0的速度(占空比)为负70%
    motor.updateMotorSpeed(1, int16_t(msg->data.data[0]*100)); // 设置电机1的速度(占空比)为正70%
  }
}
int motorStatus = 0; // 电机状态变量，0-3循环变化

void setup()
{
  sub_msg.data.capacity = 4; // 告知容量，防止内存越界
  sub_msg.data.data = array; // 赋值指针

最后要确认话题名称和数据类型不要出错，另外可以在 callback_subscription_ 加一些打印，查看回调情况。祝一切顺利。

下次提问可以按照提问模板来：https://fishros.org.cn/forum/topic/151 ，如果能补充系统版本，ROS 版本和micro-ROS版本，可以排除掉更多的问题。

harebert

@小鱼
谢谢小鱼，的确帮助我这样的新手学习了不少知识。（我看了提问格式，以后尽量按照这样来发文）

问题描述：

同本帖的问题，还未解决
我想问一下，在论坛里也没有搜索到的是当我使用agent通过Serial连接了上位机以后，还有什么方式可以在下位机上进行打印输出呢？串口监视器里似乎什么也看不到。

尝试过的解决方法：

我已经大致明白这个问题的原理。只是根据您的代码，仍然没有解决问题。
我在回调函数中设置了无论什么情况，是电机转动，但是我测试下来是回调函数没有起作用。
我也尝试用WiFi连接，连接是成功的，但是仍然无法启动回调函数。

代码

#include <Arduino.h>

#include <micro_ros_platformio.h>

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

#include <std_msgs/msg/float64.h>
#include <Esp32McpwmMotor.h>
#include <std_msgs/msg/float64_multi_array.h>

#include <WiFi.h>


Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
// 声明话题订阅者
rcl_subscription_t subscriber;
// 声明消息文件
// std_msgs__msg__Float64 sub_msg;
std_msgs__msg__Float64MultiArray sub_msg;
double array[4];

void callback_subscription_(const void *msgin)
{
  const std_msgs__msg__Float64MultiArray *msg = (const std_msgs__msg__Float64MultiArray *)msgin;
  motor.updateMotorSpeed(0, 100); //测试回调函数是否起效
  if (msg->data.size >= 2) {
    motor.updateMotorSpeed(0, int16_t(msg->data.data[0]*100)); // 设置电机0的速度(占空比)为负70%
    motor.updateMotorSpeed(1, int16_t(msg->data.data[0]*100)); // 设置电机1的速度(占空比)为正70%
  

  }

}



void setup()
{
  Serial.begin(115200);    // 初始化串口通信
  Serial1.begin(9600);




  motor.attachMotor(0, 16, 17); // 将电机0连接到引脚23和引脚22
    motor.attachMotor(1, 12, 22); // 将电机1连接到引脚12和引脚13
     motor.attachMotor(2, 25, 33); // 将电机1连接到引脚12和引脚13



sub_msg.data.capacity=4;
sub_msg.data.data=array;

  // 设置通过串口进行MicroROS通信
  set_microros_serial_transports(Serial);
  // 延时时一段时间，等待设置完成
  delay(2000);
  // 初始化内存分配器
  allocator = rcl_get_default_allocator();
  // 创建初始化选项
  rclc_support_init(&support, 0, NULL, &allocator);
  // 创建节点 topic_sub_test
  rclc_node_init_default(&node, "esp32node", "", &support);
  // 订阅者初始化
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray),
      "joyaxisTopic_0");
  // 创建执行器
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  // 为执行器添加一个订阅者
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &callback_subscription_, ON_NEW_DATA);
}

void loop()
{

  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(1000));
}

附上rqt的图

附 joyaxisTopic_0的echo

小鱼

@harebert 抱歉，上一次的代码并没有初始化彻底，因为该消息接口: std_msgs__msg__Float64MultiArray 包含layout和data两部分，上一次提供的代码里只初始化了data,并未考虑对layout进行初始化。

查看layout部分消息接口可知：

// Struct for a sequence of std_msgs__msg__MultiArrayDimension.
typedef struct std_msgs__msg__MultiArrayDimension__Sequence
{
  std_msgs__msg__MultiArrayDimension * data;
  /// The number of valid items in data
  size_t size;
  /// The number of allocated items in data
  size_t capacity;
} std_msgs__msg__MultiArrayDimension__Sequence;

其也包含了指针类型接口体成员，所以需要使用类似的方法进行初始化发方便起见，使用malloc进行初始化。

  // data 部分内存初始化
  sub_msg.data.capacity = 4;
  sub_msg.data.data = array;
  sub_msg.data.size = 0;
  // layout 部分内存初始化
  sub_msg.layout.dim.capacity = 4;
  sub_msg.layout.dim.size = 0;
  sub_msg.layout.dim.data = (std_msgs__msg__MultiArrayDimension *)malloc(sub_msg.layout.dim.capacity * sizeof(std_msgs__msg__MultiArrayDimension));
  for (size_t i = 0; i < sub_msg.layout.dim.capacity; i++)
  {
    sub_msg.layout.dim.data[i].label.capacity = 4;
    sub_msg.layout.dim.data[i].label.size = 0;
    sub_msg.layout.dim.data[i].label.data = (char *)malloc(sub_msg.layout.dim.data[i].label.capacity * sizeof(char));
  }

对于回调打印或者使用第二个串口的方法，这里有三个方案：

使用串口+无线蓝牙（ESP32支持蓝牙，可以通过蓝牙进行数据输出，手机APP配合查看日志）
2.使用串口+WIFI（通过WIFI连接-miros-ROS,串口用于日志查看）
3.使用额外的串口（需要初始化为其他引脚，还需要一个USB转TTL工具连接，最后的代码提供了初始化方法）

上述三个方法，推荐程度：2>1>3，1和2可以在FishBot配套程序中找到使用方法。

最后附上代码和测试结果（调试通过的）：

#include <Arduino.h>

#include <micro_ros_platformio.h>

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

#include <std_msgs/msg/float64.h>
#include <std_msgs/msg/float64_multi_array.h>

#include <WiFi.h>

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
// 声明话题订阅者
rcl_subscription_t subscriber;
// 声明消息文件
// std_msgs__msg__Float64 sub_msg;
std_msgs__msg__Float64MultiArray sub_msg;
double array[4];

void callback_subscription_(const void *msgin)
{
  const std_msgs__msg__Float64MultiArray *msg = (const std_msgs__msg__Float64MultiArray *)msgin;
  Serial1.printf("size=%d,data:[%f,%f]\n", sub_msg.data.size, sub_msg.data.data[0], sub_msg.data.data[1]);
  digitalWrite(2, !digitalRead(2)); // 闪烁
}

void setup()
{
  Serial.begin(115200);                      // 初始化串口通信
  Serial1.begin(115200, SERIAL_8N1, 17, 16); // 加个串口
  pinMode(2, OUTPUT);                        // 设置2号引脚模式为OUTPUT模式

  // data 部分内存初始化
  sub_msg.data.capacity = 4;
  sub_msg.data.data = array;
  sub_msg.data.size = 0;
  // layout 部分内存初始化
  sub_msg.layout.dim.capacity = 4;
  sub_msg.layout.dim.size = 0;
  sub_msg.layout.dim.data = (std_msgs__msg__MultiArrayDimension *)malloc(sub_msg.layout.dim.capacity * sizeof(std_msgs__msg__MultiArrayDimension));
  for (size_t i = 0; i < sub_msg.layout.dim.capacity; i++)
  {
    sub_msg.layout.dim.data[i].label.capacity = 4;
    sub_msg.layout.dim.data[i].label.size = 0;
    sub_msg.layout.dim.data[i].label.data = (char *)malloc(sub_msg.layout.dim.data[i].label.capacity * sizeof(char));
  }

  // 设置通过串口进行MicroROS通信
  set_microros_serial_transports(Serial);
  // 延时时一段时间，等待设置完成
  delay(2000);
  // 初始化内存分配器
  allocator = rcl_get_default_allocator();
  // 创建初始化选项
  rclc_support_init(&support, 0, NULL, &allocator);
  // 创建节点 topic_sub_test
  rclc_node_init_default(&node, "esp32node", "", &support);
  // 订阅者初始化
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray),
      "joyaxisTopic_0");
  // 创建执行器
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  // 为执行器添加一个订阅者
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &callback_subscription_, ON_NEW_DATA);
}

void loop()
{

  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(1000));
}

发布指令

ros2 topic pub /joyaxisTopic_0 std_msgs/msg/Float64MultiArray "{layout: {dim: [{label: '', size: 0, stride: 0}], data_offset: 0}, data: [1.0, 2.0]}"

Serial1打印：

size=2,data:[1.000000,2.000000]
size=2,data:[1.000000,2.000000]
size=2,data:[1.000000,2.000000]
size=2,data:[1.000000,2.000000]
size=2,data:[1.000000,2.000000]
size=2,data:[1.000000,2.000000]
size=2,data:[1.000000,2.000000]

对了，如果非必要不要截图，终端打印等最好使用文本进行复制粘贴。

harebert

@小鱼
谢谢。问题最终解决。
现在已经能传输4个摇杆参数，并且控制小车的各种动作了。

响应函数的代码如下，给以后遇到这个问题的朋友做个参考：

void callback_subscription_(const void *msgin)
{
  const std_msgs__msg__Float64MultiArray *msg = (const std_msgs__msg__Float64MultiArray *)msgin;
  // motor.updateMotorSpeed(0, 100);
  if (msg->data.size >= 4) {
    //data[0] 为左摇杆的左右方向
    //data[1] 为左摇杆的上下方向
    //data[2] 为右摇杆的左右方向
    //data[3] 为右摇杆的上下方向
        // 获取摇杆的输入
    double leftHorizontal = -msg->data.data[0]; // 左摇杆左右
    double leftVertical = msg->data.data[1];   // 左摇杆上下
    double rightHorizontal = -msg->data.data[2]; // 右摇杆左右

    /// 计算每个电机的速度
    double motorSpeed0 = (leftVertical - leftHorizontal + rightHorizontal) * 100;
    double motorSpeed1 = (leftVertical + leftHorizontal - rightHorizontal) * 100;
    double motorSpeed2 = (leftVertical - leftHorizontal - rightHorizontal) * 100;
    double motorSpeed3 = (leftVertical + leftHorizontal + rightHorizontal) * 100;

     // 更新电机速度，确保速度值在-100到100之间
    motor.updateMotorSpeed(0, static_cast<int16_t>(std::max(std::min(motorSpeed0, 100.0), -100.0)));
    motor.updateMotorSpeed(1, static_cast<int16_t>(std::max(std::min(motorSpeed1, 100.0), -100.0)));
    motor.updateMotorSpeed(2, static_cast<int16_t>(std::max(std::min(motorSpeed2, 100.0), -100.0)));
    motor.updateMotorSpeed(3, static_cast<int16_t>(std::max(std::min(motorSpeed3, 100.0), -100.0)));

  }

}

问题描述：

1.前进、转向、平移都没有问题，现在的问题是前进叠加转向、或者平移的时候，电机转动一会就不动了。
2.关于Serial输出的问题，当我在platformio.ini加载了board_microros_transport = wifi以后，在main.cpp中的set_microros_serial_transports(Serial);就会报【未定义标识符 "set_microros_serial_transports"】

尝试：

问题1，我估计是不是电机过载？
问题2，我想是不是响应的在platformio.ini中要写入关于Serial的相关库语句呢？

小鱼

@harebert 容许我小小的吐槽一下，尝试不是我估计和我想，而是我根据估计去做了什么测试，结果是什么。比如你觉得是电机过载，可以尝试降低速度再测试。

现在正式回答一下：

先排除通信问题，另外问题描述中，没有讲到如何供电，电池电量等信息，所以需要你进一步排查。
2.既然你设置了使用wifi传输，在代码里设置通过serial传输，这是很明显的冲突，请根据动手学ROS2或者小车配套教程中的使用方法，设置通过WIFI传输。

harebert

@小鱼

Serial的问题解决了。谢谢。的确我是没有搞清楚传输的代码。
我使用的是外接电源供电同时打开了电池的开关。

通过串口的输出，在停转的时候，我获得了以下信息。

代码：

double leftHorizontal = msg->data.data[0]; // 左摇杆左右
    double leftVertical = msg->data.data[1];   // 左摇杆上下
    double rightHorizontal = -msg->data.data[2]; // 右摇杆左右

    /// 计算每个电机的速度
    double motorSpeed0 = (leftVertical - leftHorizontal + rightHorizontal) * 100;
    double motorSpeed1 = (leftVertical + leftHorizontal - rightHorizontal) * 100;
    double motorSpeed2 = (leftVertical - leftHorizontal - rightHorizontal) * 100;
    double motorSpeed3 = (leftVertical + leftHorizontal + rightHorizontal) * 100;

     // 更新电机速度，确保速度值在-100到100之间
    motor.updateMotorSpeed(0, static_cast<int16_t>(std::max(std::min(motorSpeed0, 100.0), -100.0)));
    motor.updateMotorSpeed(1, static_cast<int16_t>(std::max(std::min(motorSpeed1, 100.0), -100.0)));
    motor.updateMotorSpeed(2, static_cast<int16_t>(std::max(std::min(motorSpeed2, 100.0), -100.0)));
    motor.updateMotorSpeed(3, static_cast<int16_t>(std::max(std::min(motorSpeed3, 100.0), -100.0)));
    Serial.println(motorSpeed0);
    Serial.println(static_cast<int16_t>(std::max(std::min(motorSpeed0, 100.0), -100.0)));

错误信息：

ets Jul 29 2019 12:21:46

rst:0x8 (TG1WDT_SYS_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT)
configsip: 0, SPIWP:0xee
clk_drv:0x00,q_drv:0x00,d_drets Jul 29 2019 12:21:46

rst:0x8 (TG1WDT_SYS_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT)
configsip: 0, SPIWP:0xee
clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00
mode:DIO, ets Jul 29 2019 12:21:46

rst:0x8 (TG1WDT_SYS_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT)
configsip: 0, SPIWP:0xee
clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00
mode:DIO, clock div:1
load:0x3fff0030,len:1184
load:0x40078000,len:13260
load:0x40080400,len:3028
entry 0x400805e4

小鱼

@harebert 这个问题已经不属于消息接口问题，请单独附上完整代码和上下文，单独发帖。