如何从rosserial arduino发布Int16MultiArray

Question

我正在尝试发布ros包mecanum_drive：https://github.com/dudasdavid/mecanum_drive的Int16MultiArray。

我的问题是，我似乎不能从我的arduino发布数组。(我使用的是Teensy 4.1)

#include <std_msgs/Int16MultiArray.h>

ros::NodeHandle nh;
std_msgs::Int16MultiArray wheel_ticks;
ros::Publisher wheel_ticks_pub("wheel_ticks", &wheel_ticks);
void setup() {
  nh.getHardware()->setBaud(115200); //was 115200
  nh.initNode();              // init ROS
  nh.advertise(wheel_ticks_pub);
}

void loop() {
  // put your main code here, to run repeatedly:
 //I have tried the code below which uploads to the arduino, but rostopic then says that it dosnt contain any data
 /*
 short value[4] = {0,100,0,0};
 wheel_ticks.data = value;
 */

 //I also tryed the code below which uploads, but then the teensy looses its serial port (arduino port says"[no_device] Serial(Teensy4.1)": 
 /*
 wheel_ticks.data[0] = 10;
 wheel_ticks.data[1] = 5;
 */

 //below gives this error: cannot convert '<brace-enclosed initializer list>' to 'std_msgs::Int16MultiArray::_data_type* {aka short int*}' in assignment
 /*
 wheel_ticks.data = {0,0,0,1};
 */
 wheel_ticks_pub.publish(&wheel_ticks);

nh.spinOnce();      
}

我试过的所有东西要么没有上传，要么上传了，但是序列被搞乱了，或者上传了，rostopic echo说它是空的。

谢谢你看这篇文章，希望你能帮上忙！

Answer 1

rosserial的一个非常特殊的限制是数组有一个专门用于数据长度的额外字段。这是必需的，因为数据字段是作为指针实现的，因此没有真正好的方法来获取数据长度。消息类型实际上如下所示

class Int16MultiArray{
    Header header;
    int data_length;
    int16_t * data;
};

因此，您所要做的就是在发送消息之前设置数据字段

#include <std_msgs/Int16MultiArray.h>

ros::NodeHandle nh;
std_msgs::Int16MultiArray wheel_ticks;
ros::Publisher wheel_ticks_pub("wheel_ticks", &wheel_ticks);
void setup() {
  nh.getHardware()->setBaud(115200); //was 115200
  nh.initNode();              // init ROS
  nh.advertise(wheel_ticks_pub);
}
void loop() {
 
  short value[4] = {0,100,0,0};
  wheel_ticks.data = value;
  wheel_ticks.data_length = 4;
 
  wheel_ticks_pub.publish(&wheel_ticks);

  nh.spinOnce();      
}