rosserial segfault when processing Int16MultiArray message

asked 2016-08-06 16:16:22 -0600

Piachnp gravatar image

Hi, I am having some troubles on the Linux side of rosserial when trying to process an Int16MultiArray message. The setup is as follows:

  • An arduino MEGA publishes 24 integer values into a Int16MultiArray message.
  • I want to log that data into a csv file on the PC side of things.

This is how the code looks like on the Arduino:

#include <ros.h>
#include <std_msgs/Int16MultiArray.h>
#include <std_msgs/MultiArrayDimension.h>
#include <std_msgs/MultiArrayLayout.h>

ros::NodeHandle nh;
std_msgs::Int16MultiArray readings_msg;
std_msgs::MultiArrayDimension myDim;
std_msgs::MultiArrayLayout myLayout;
ros::Publisher pub_sensor("optic_sensor", &readings_msg);
int idx=0;

void setup() {
  nh.getHardware()->setBaud(115200);
  nh.initNode();

  myDim.label = "readings";
  myDim.size = 24;
  myDim.stride = 24;
  myLayout.dim = (std_msgs::MultiArrayDimension *) malloc(sizeof(std_msgs::MultiArrayDimension) * 1);
  myLayout.dim[0] = myDim;
  myLayout.data_offset = 0;
  readings_msg.layout = myLayout;
  readings_msg.data = (int *)malloc(sizeof(int)*24);
  readings_msg.data_length = 24;
  nh.advertise(pub_sensor);

  //Set output pins to control LEDs
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT); 
  pinMode(LED3, OUTPUT);
}

void loop() {
    for(int i=0;i<8;i++)
    {
      switch_handler(i+1);
      readings_msg.data[idx] = averageAnalog(0,2);
      readings_msg.data[idx+1] = averageAnalog(1,2);
      readings_msg.data[idx+2] = averageAnalog(2,2);
      idx=idx+3;
    }
    if(idx == 24)
    {
      idx=0;
      pub_sensor.publish( &readings_msg );
      nh.spinOnce();  
    }
}

On the PC side, the function failing are these:

void Indenter::sample_response_test()
{
    // Open a csv file to log data
    ofstream myfile;    
    myfile.open ("filename.csv", ios::out | ios::trunc );
    std_msgs::Int16MultiArray::ConstPtr msg = ros::topic::waitForMessage<std_msgs::Int16MultiArray>("arduino_sample/optic_sensor",*nh,ros::Duration(1));
    log_msg_to_csv(myfile, msg);
    cout<<"Closing file"<<endl;
    myfile.close();
}

void Indenter::log_msg_to_csv (ofstream& myfile, const std_msgs::Int16MultiArray::ConstPtr& msg)
{
    //These 2 lines are just debugging tests
    myfile << msg->data[0]<<msg->data[1];       //THIS WORKS, access to data doesn't present problem
    cout<<"dim 0 size is: "<<msg->layout.dim[0].size<<endl;     //THIS SEGFAULTS, can't access the size of the array
    //This is the real functionality of the function
    for(int i=0; i< msg->layout.dim[0].size ;i++)
    {
        myfile << msg->data[i];
        if(i != msg->layout.dim[0].size-1)
            myfile << ";";
        else
            myfile << endl;
    } 
}

The problem could be either on the arduino side when I construct the message, or on the PC side trying to access. My money is on the arduino side to be honest. Note that the message is published successfully, and I can see it by using rostopic, and the data can be accessed as well on the PC side, by reading the msg.data field. However, since it is a 24 integer array, when you try to loop through it 24 times to get all the readings, you need to read the size information from the message. While I could hardcode the value 24, I'd like to figure out what is it that I'm doing wrong.

Thanks in advance for your help!

edit retag flag offensive close merge delete