Rosserial Arduino Proper Use Of Float32MultiArray

edit

Hi I am struggling for a week for the proper use of Float32MultiArray. I am using an Arduino mega and I have an IMU which sends the data over Serial2 for yaw, pitch and roll data.

I tested the IMU code and it is working well. Then I tried to publish the data to ROS using Float32MultiArray and that's where the problem comes in.

With the below code, I can see the IMU data is reading but the "/imu_ypr" topic is not published. If I comment out the "imu_ypr_msg" layout code, I can see the topic "/imu_ypr" using "rostopic list" but the IMU is not reading the data. I guess it is something related to the memory allocation but I cannot figure it out. Any help is appreciated! Thanks!

#include <ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/MultiArrayDimension.h>
#include <std_msgs/String.h>

float yaw   = 0;
float pitch = 0;
float  roll  = 0;

int index = 0;
int temp;
byte data;
byte imu_buffer[20];

ros::NodeHandle nh;
std_msgs::Float32MultiArray imu_ypr_msg;
ros::Publisher pub_imu_ypr("imu_ypr", &imu_ypr_msg);

void setup() {
  // put your setup code here, to run once:
  nh.initNode();
  imu_setup();

  /*imu_ypr_msg.layout.dim_length = 1;
  imu_ypr_msg.layout.dim[0].label = "imu";
  imu_ypr_msg.layout.dim[0].size = 3;
  imu_ypr_msg.layout.dim[0].stride = 3;
  imu_ypr_msg.layout.data_offset = 0;
  imu_ypr_msg.layout.dim = (std_msgs::MultiArrayDimension *)malloc(sizeof(std_msgs::MultiArrayDimension) * 2);      */
  imu_ypr_msg.data_length = 3;
  imu_ypr_msg.data = (float *)malloc((sizeof(float))*imu_ypr_msg.data_length * 2);  
  nh.advertise(pub_imu_ypr);

}

void loop() {
  // put your main code here, to run repeatedly:
  imu_check();
  imu_ypr_msg.data[0] = yaw;
  imu_ypr_msg.data[1] = pitch;
  imu_ypr_msg.data[2] = roll;
  pub_imu_ypr.publish(&imu_ypr_msg);
  nh.spinOnce();
}

void imu_setup() {
  Serial2.begin(115200);
}

void imu_check() {
  if(Serial2.available() > 0) {
    //data = imuSerial.read();
    data = Serial2.read();

    if(data == 0xA5) {
      index = 0;
      imu_buffer[index++] = data; 
    } else if(index==1 && data==0x5A) {
      imu_buffer[index++] = data;       
    } else if(index==2 && data==0x12) {
      imu_buffer[index++] = data;       
    } else if(index==3 && data==0xA1) {
      imu_buffer[index++] = data;       
    } else if(index >= 4) {
      imu_buffer[index++] = data;             
    }
    if(index > 9) {
      nh.loginfo("IMU data found");

      temp = 0;
      temp = imu_buffer[4];
      temp <<= 8;
      temp |= imu_buffer[5];
      if(temp & 0x8000) {
        temp = 0 - (temp & 0x7fff);
      } else {
        temp = (temp & 0x7fff);
      }
      yaw = (float)temp / 10.0f;

      temp = 0;
      temp = imu_buffer[6];
      temp <<= 8;
      temp |= imu_buffer[7];      
      if(temp & 0x8000) {
        temp = 0 - (temp & 0x7fff);
      } else {
        temp = (temp & 0x7fff);
      }
      pitch = (float)temp / 10.0f;

      temp = 0;
      temp = imu_buffer[8];
      temp <<= 8;
      temp |= imu_buffer[9];      
      if(temp & 0x8000) {
        temp = 0 - (temp & 0x7fff);
      } else {
        temp = (temp & 0x7fff);
      }
      roll = (float)temp / 10.0f;
      index = 0;
      /*
      Serial.print("yaw: ");
      Serial.print(yaw);
      Serial.print(" pitch: ");
      Serial.print(pitch);
      Serial.print(" roll: ");
      Serial.println(roll);   
      */
    }
  }
}

I gave up on using multiarray and I switched to custom message instead.