Added new prameters for the IMU covariance.

Set the sensor units to rad/s and m/s/s.

cfg/Bno055Imu.cfg

@@ -146,6 +146,9 @@ gyro.add("gyro_pwr_mode",          int_t,     SensorLevels.RECONFIGURE_STOP,   "
 gyro.add("mag_rate",               int_t,     SensorLevels.RECONFIGURE_STOP,   "Magnetometer rate",              3,        0,    7, edit_method=mag_rate)
 gyro.add("mag_op_mode",            int_t,     SensorLevels.RECONFIGURE_STOP,   "Magnetometer operation mode",    8,        0,    24, edit_method=mag_op_mode)
 gyro.add("mag_pwr_mode",           int_t,     SensorLevels.RECONFIGURE_STOP,   "Magnetometer power mode",        0,        0,    96, edit_method=mag_pwr_mode)
+gen.add("orientation_cov",         double_t,  SensorLevels.RECONFIGURE_RUNNING,"Orientation covariance",  0.001,      0.0,  1.0)
+gen.add("angular_velocity_cov",    double_t,  SensorLevels.RECONFIGURE_RUNNING,"Angular velocity covariance",  0.001,      0.0,  1.0)
+gen.add("linear_acceleration_cov", double_t,  SensorLevels.RECONFIGURE_RUNNING,"linear acceleration covariance",  0.001,      0.0,  1.0)
 #gen.add("velocity_scale_factor",  double_t,  SensorLevels.RECONFIGURE_STOP,   "Maximum velocity scale factor",  0.5,      0.0,  1.0)
 
 exit(gen.generate(PACKAGE, "Bno055ImuDriver", "Bno055Imu"))

include/bno055_imu_ros_driver.h

@@ -156,6 +156,9 @@ class Bno055ImuDriver : public iri_base_driver::IriBaseDriver
     std::vector<double> get_raw_accelerometer(void);
     std::vector<double> get_raw_magnetometer(void);
     std::vector<double> get_raw_gyroscope(void);
+    double get_orientation_cov(void);
+    double get_angular_velocity_cov(void);
+    double get_linear_acceleration_cov(void);
     bool is_imu_calibrated(void);
     std::string get_frame_id(void);
    /**

src/bno055_imu_ros_driver.cpp

@@ -54,6 +54,7 @@ bool Bno055ImuDriver::startDriver(void)
   {
     try{
       this->imu_device->set_operation_mode(config_mode);
+      this->imu_device->set_sensor_units(false,true);
       this->imu_device->load_calibration(this->config_.cal_filename);
       this->imu_device->set_data_rate(this->config_.update_rate);
       this->imu_device->set_accel_config(this->config_.accel_range,this->config_.accel_bandwidth,this->config_.accel_pwr_mode);
@@ -194,6 +195,21 @@ std::vector<double> Bno055ImuDriver::get_raw_gyroscope(void)
     return std::vector<double>();
 }
 
+double Bno055ImuDriver::get_orientation_cov(void)
+{
+  return this->config_.orientation_cov;
+}
+
+double Bno055ImuDriver::get_angular_velocity_cov(void)
+{
+  return this->config_.angular_velocity_cov;
+}
+
+double Bno055ImuDriver::get_linear_acceleration_cov(void)
+{
+  return this->config_.linear_acceleration_cov;
+}
+
 bool Bno055ImuDriver::is_imu_calibrated(void)
 {
   bool accel_cal,mag_cal,gyro_cal;

src/bno055_imu_ros_driver_node.cpp

@@ -70,9 +70,15 @@ void Bno055ImuDriverNode::mainNodeThread(void)
       this->imu_Imu_msg_.orientation.y=quaternion[1];
       this->imu_Imu_msg_.orientation.z=quaternion[2];
       this->imu_Imu_msg_.orientation.w=quaternion[3];
+      this->imu_Imu_msg_.orientation_covariance[0]=this->driver_.get_orientation_cov();
+      this->imu_Imu_msg_.orientation_covariance[4]=this->driver_.get_orientation_cov();
+      this->imu_Imu_msg_.orientation_covariance[8]=this->driver_.get_orientation_cov();
       this->imu_Imu_msg_.angular_velocity.x=gyro[0];
       this->imu_Imu_msg_.angular_velocity.y=gyro[1];
       this->imu_Imu_msg_.angular_velocity.z=gyro[2];
+      this->imu_Imu_msg_.angular_velocity_covariance[0]=this->driver_.get_angular_velocity_cov();
+      this->imu_Imu_msg_.angular_velocity_covariance[4]=this->driver_.get_angular_velocity_cov();
+      this->imu_Imu_msg_.angular_velocity_covariance[8]=this->driver_.get_angular_velocity_cov();
       if(op_mode>=imu_mode)
       {
         this->imu_Imu_msg_.linear_acceleration.x=linear_accel[0];
@@ -85,6 +91,9 @@ void Bno055ImuDriverNode::mainNodeThread(void)
         this->imu_Imu_msg_.linear_acceleration.y=accel[1];
         this->imu_Imu_msg_.linear_acceleration.z=accel[2];
       }
+      this->imu_Imu_msg_.linear_acceleration_covariance[0]=this->driver_.get_linear_acceleration_cov();
+      this->imu_Imu_msg_.linear_acceleration_covariance[4]=this->driver_.get_linear_acceleration_cov();
+      this->imu_Imu_msg_.linear_acceleration_covariance[8]=this->driver_.get_linear_acceleration_cov();
       this->imu_publisher_.publish(this->imu_Imu_msg_);
     }catch(CException &e){  
       ROS_WARN_STREAM(e.what());