rosserial_arduino: publishing a ROS message corrupts other messages
Hi all
I'm using an an Arduino Mega2560 to interface a MPU6050 using Jeff Rowberg's MPU6050_6Axis_MotionApps20 lib. I'm also publishing data from ultrasound sensors (with Tim Eckel's NewPing.h lib) in a std_msgs::UInt16 message, and left and right encoders count data (attached to interrupts) in two std_msgs::UInt64 messages. They are working fine, I can read the data using "rostopic echo" in the host computer (a NVidia TX1). I'm publishing Yaw-Pitch-Roll data from the IMU using a geometry_msgs::Vector3 message that also appears to work fine. However when I publish a sensor_msgs::Imu message, this message itself seems to be correct (I can see it with right values using "rostopic echo" in the host), but both encoder messages get corrupted, showing a huge value for the encoder count. Watching the encoder messages during some time, every now and then the correct value is still shown, but most of the time the corrupted value appears.
I've read old issues about overflowing the Arduino serial buffer but I've tried to increase the bufferfrom 512 to 1024 bytes and the problem still persists. In any case the Imu message itself is being transmitted (I think it's a bit over 300 bytes long).
After much trial and error I've reached a point where I can fill in and publish the complete sensor_msgs::Imu message EXCEPT for a single field: header.stamp.sec. If I write a 0 value here the encoders messages still appear to be correct, but writing any other integer value corrupts the encoders messages. Writing to 0 other fields (i.e. nsec) instead of sec also causes the corruption. This corruption isn't random, it seems that the integer value I write in the sec field ends up multiplied by 2^32 and added to the encoder count. It seems that the value written in the sec field of the Imu message ends up pre-pended to the data field in both encoders messages. The Imu message is always correctly transmitted, and only the encoder messages are affected (sonar messages are always correctly transmitted also).
Anyone had a similar problem?
