ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

After changing the buffer size, error again in publishing a array msgs with large data length 240in Arduino?

asked 2014-02-04 21:09:37 -0500

bisimai111 gravatar image

updated 2014-04-20 14:09:45 -0500

ngrennan gravatar image

Dear all, I am trying to publish a msgs with large data size on Arduino. I am using Float32MultiArray msgs. Thanks for the solutions from http://answers.ros.org/question/12545... So, I changed the buffer size from 280 to 512. However, when i increase the data_length more than 30, it shows an error in the terminal:

[INFO] [WallTime: 1391356801.758318] Failed Packet Flags

after the command rosrun rosserial_python serial_node.py /dev/ttyACM0 (the code as attached) again. If I keep the size below 30, the msgs can be published normally.

I want to pass 240 data size in one msgs array like array[240]..

Could you help me and give me some suggestion , please? thank you!!

#include <ros.h>
#include <ros time.h>
#include <std_msgs/Float32MultiArray.h>


ros::NodeHandle  nh;
std_msgs::Float32MultiArray range_msg;
ros::Publisher pub_range("/test",  &range_msg);

void setup()
{
  nh.initNode();

  range_msg.data_length=100;

  nh.advertise(pub_range);

}

long range_time;

void loop()
{
  //publish the adc value every 50 milliseconds
  //since it takes that long for the sensor to stabilize

  if ( millis() >= range_time ) {

    for(int i=0;i<100;i++)
    {
      range_msg.data[i]=0.01222331221;
    }

    pub_range.publish(&range_msg);

    range_time =  millis() + 50;
  }

  nh.spinOnce();
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-09-16 07:31:01 -0500

rreignier gravatar image

As explained in the rosserial_wiki on section 2.4 Arrays (and 2.3 Strings), you should store date in a local array and then give the pointer to that array.

So with this code, I could receive 240 values:

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

ros::NodeHandle  nh;
std_msgs::Float32MultiArray range_msg;
constexpr size_t DATA_SIZE = 240;
float data[DATA_SIZE];
ros::Publisher pub_range("/test",  &range_msg);

void setup()
{
delay(1000);
nh.initNode();

range_msg.data_length = DATA_SIZE;

nh.advertise(pub_range);

}

long range_time;

void loop()
{
//publish the adc value every 50 milliseconds
//since it takes that long for the sensor to stabilize

if ( millis() >= range_time ) {

    for(int i=0;i<DATA_SIZE;i++)
    {
    data[i]=i;
    }

    range_msg.data = data;

    pub_range.publish(&range_msg);

    range_time =  millis() + 50;
}

nh.spinOnce();
}

But I have noticed that I do not get the messages at 20 Hz. Setting a higher baudrate (before initNode) helps a bit:

 nh.getHardware()->setBaud(500000);

And of course, I have increased the buffers sizes to 2048.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2014-02-04 21:09:37 -0500

Seen: 699 times

Last updated: Sep 16 '18