如何从 rosserial arduino 发布 Int16MultiArray

Question

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

我的问题是我似乎无法从我的 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();      
}

如何从 rosserial arduino 发布 Int16MultiArray

How to publish Int16MultiArray from rosserial arduino

c++

arduino

ros