How to publish Int16MultiArray from rosserial arduino

user_16504987 0
Viewed 577

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!

ros arduino c++
1 Answers

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();      
}
user_119362290
Related