From aaf8e36c478c62fed451ad0ba20418101bb35932 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 8 Jun 2021 19:05:50 +0200
Subject: [PATCH] Added new prameters for the IMU covariance. Set the sensor
units to rad/s and m/s/s.
---
cfg/Bno055Imu.cfg | 3 +++
include/bno055_imu_ros_driver.h | 3 +++
src/bno055_imu_ros_driver.cpp | 16 ++++++++++++++++
src/bno055_imu_ros_driver_node.cpp | 9 +++++++++
4 files changed, 31 insertions(+)
diff --git a/cfg/Bno055Imu.cfg b/cfg/Bno055Imu.cfg
index 761a687..6faa9d5 100755
--- a/cfg/Bno055Imu.cfg
+++ b/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"))
diff --git a/include/bno055_imu_ros_driver.h b/include/bno055_imu_ros_driver.h
index aa9a6bb..1e3315a 100644
--- a/include/bno055_imu_ros_driver.h
+++ b/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);
/**
diff --git a/src/bno055_imu_ros_driver.cpp b/src/bno055_imu_ros_driver.cpp
index 002284c..3dfa702 100644
--- a/src/bno055_imu_ros_driver.cpp
+++ b/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;
diff --git a/src/bno055_imu_ros_driver_node.cpp b/src/bno055_imu_ros_driver_node.cpp
index 47e2365..be62f20 100644
--- a/src/bno055_imu_ros_driver_node.cpp
+++ b/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());
--
GitLab