Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino
I am running ros kinetic and my rosserial works fine for all programs except one( in which i get above error).
I have tried all the techniques suggested but even then it doesnt work.( baudrate is fine , i tried to plugin usb a lot of times, rosserial works fine for other programs) but still i am getting the same error for only one program. What is the solution for this?
Here is my code for arduino:
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#include <ros.h>
#include <sensor_msgs/Imu.h>
ros::NodeHandle nh;
sensor_msgs::Imu Imu_msg;
ros::Publisher pub_Imudata("Imu_data",&Imu_msg);
MPU6050 mpu;
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define LED_PIN 13
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
byte statusLed =13;
byte sensorInterrupt=0;
byte sensorPin=2;
double calibrationFactor=4.5;
volatile byte pulseCount;
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
//changed this
Serial.begin(115200);
while (!Serial);
nh.initNode();
nh.advertise(pub_Imudata);
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// wait for ready
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
if (devStatus == 0)
{
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(1 , dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else
{
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
void loop()
{
Imu_msg.header.seq++;
Imu_msg.header.stamp = nh.now();
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize) {}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
mpu.resetFIFO();
}
else if (mpuIntStatus & 0x02)
{
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
Imu_msg.orientation.w = q.w;
Imu_msg.orientation.x = q.x;
Imu_msg ...
In my experience, this can be a software mismatch (as stated in the error) or a connection problem. I've never known it to be a problem with code.
I don't think its a software mismatch because i tried to run other codes and they run fine.