 // IMU and Magnetometer
 LSM6 imu;
 LIS3MDL mag;
+bool imu_err = false;
+bool mag_err = false;
 
 char debug_str[80];
 

  Wire.setDefaultTimeout(200000); // 200ms

  // Setup IMU and magenetometer that use I2C
  if (!imu.init())
  {
    Serial.println("Failed to detect and initialize IMU!");
    while (1);
  }
  imu.enableDefault();
  
  if (!mag.init())
  {
    Serial.println("Failed to detect and initialize magnetometer!");
    while (1);
  }

  mag.enableDefault();

  ros_encoder_pub.publish(&ros_debug_msg);
  
  // Read the IMU
  if (imu_err) {
    ros_debug_msg.data = "IMU failure!";
  } else {
    long i2c_start_time = millis();
    imu.read();
    long i2c_duration = millis() - i2c_start_time;
  
    snprintf(debug_str, sizeof(debug_str), "A: %6d %6d %6d    G: %6d %6d %6d   I2C Duration: %ld",
      imu.a.x, imu.a.y, imu.a.z,
      imu.g.x, imu.g.y, imu.g.z,
      i2c_duration);
    ros_debug_msg.data = debug_str;
  }
  ros_imu_pub.publish(&ros_debug_msg);

  // Read the Magnetometer
  if (mag_err) {
    ros_debug_msg.data = "Magnetometer failure!";
  } else {
    long i2c_start_time = millis();
    mag.read();
    long i2c_duration = millis() - i2c_start_time;
  
    snprintf(debug_str, sizeof(debug_str), "M: %6d %6d %6d   I2C Duration: %ld",
      mag.m.x, mag.m.y, mag.m.z,
      i2c_duration);
    ros_debug_msg.data = debug_str;
  }
  ros_magnetometer_pub.publish(&ros_debug_msg);

  // Send all the ROS messages
  ros_nh.spinOnce();
}
