How to publish Int16MultiArray from rosserial arduino - c++

Question

I am trying to publish an Int16MultiArray for the ros package mecanum_drive: https://github.com/dudasdavid/mecanum_drive
My issue is that I cant seem to publish the array from my arduino. (I am using 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();
}
Everything I have tried has either not uploaded, uploaded but with serial being messed up, or with it uploading and rostopic echo saying it is empty.
Thanks for looking at this, I hope you can help!

Answer 1

A very specific limitation of rosserial is arrays have an extra field specifically for data length. This is needed since the data field is implemented as a pointer, thus having no real good way to get data length. The message type actually looks like this
class Int16MultiArray{
Header header;
int data_length;
int16_t * data;
};
So, all you have to do is set the data field before sending a message
#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();
}