rosserial arduino losing sync
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.
You seem to only call
nh.spinOnce()
ifdmpReady == true
. That might not be what you want. I'd callspinOnce()
always, and only process IMU data when it is available.