rosserial arduino losing sync

edit

Dear community,

I have used ROS in the past, but it was for different applications.

Going straight to the point, I am building an autonomous hexapod (with lidars, depth cameras etc.) but I am struggling to have the communication with arduino working properly.

The problem seems to be linked to the calculations I do in order to obtain the IMU readings.

If I comment out the function responsible for retrieving the new data, the publishing from arduino, and the echo I do on the topic of interest, can loop forever.

If I retrieve IMU readings in the main loop the command:

rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200

fails after an arbitrary time.

Here I list the code for the loop and the function I am using in order to retrieve the readings:

void loop() {

if (!dmpReady) return;



if (state == "Idle" && millis() - t0 >= 80){

    updateMeasurements();

    delay(20);

    pub_imu.publish(&imuMeasure);

    t0 = millis();

}


nh.spinOnce();}


void updateMeasurements(){

  mpu.resetFIFO();
  while (!mpuInterrupt && fifoCount < packetSize) {
      if (mpuInterrupt && fifoCount < packetSize) {
          fifoCount = mpu.getFIFOCount();
      }  
  }
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();


  fifoCount = mpu.getFIFOCount();
  while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

  mpu.getFIFOBytes(fifoBuffer, packetSize);


  fifoCount -= packetSize;


  mpu.dmpGetQuaternion(&q, fifoBuffer);

  imuMeasure.w = q.w;
  imuMeasure.x = q.x;
  imuMeasure.y = q.y;
  imuMeasure.z = q.z;}

As I said, after an arbitrary time I start to get error messages from the rosserial python terminal like:

[ERROR] [1546970875.729310]: Lost sync with device, restarting...
[ERROR] [1546970890.731936]: Lost sync with device, restarting...
[ERROR] [1546970905.734680]: Lost sync with device, restarting...

At this point, in order to get things to work properly again, I have to reset the Arduino (mega).

I beg for your help, I am not getting out this it seems.