Added a MagneticField topic publisher.

Added functions to get the raw sensor data.
Published all available data depending on the operational mode.

include/bno055_imu_ros_driver.h

@@ -148,10 +148,14 @@ class Bno055ImuDriver : public iri_base_driver::IriBaseDriver
     // here define all bno055_imu_driver interface methods to retrieve and set
     // the driver parameters
     std::string get_new_data_event_id(void);
+    op_mode_t get_operation_mode(void);
     std::vector<double> get_orientation_quat(void);
     std::vector<double> get_orientation_euler(void);
     std::vector<double> get_linear_accel(void);
     std::vector<double> get_gravity(void);
+    std::vector<double> get_raw_accelerometer(void);
+    std::vector<double> get_raw_magnetometer(void);
+    std::vector<double> get_raw_gyroscope(void);
     bool is_imu_calibrated(void);
     std::string get_frame_id(void);
    /**

include/bno055_imu_ros_driver_node.h

@@ -29,6 +29,7 @@
 #include "bno055_imu_ros_driver.h"
 
 // [publisher subscriber headers]
+#include <sensor_msgs/MagneticField.h>
 #include <sensor_msgs/Imu.h>
 
 // [service client headers]
@@ -58,6 +59,9 @@ class Bno055ImuDriverNode : public iri_base_driver::IriBaseNodeDriver<Bno055ImuD
 {
   private:
     // [publisher attributes]
+    ros::Publisher magnetometer_publisher_;
+    sensor_msgs::MagneticField magnetometer_MagneticField_msg_;
+
     ros::Publisher imu_publisher_;
     sensor_msgs::Imu imu_Imu_msg_;
 

src/bno055_imu_ros_driver.cpp

@@ -11,6 +11,7 @@ Bno055ImuDriver::Bno055ImuDriver(void)
 bool Bno055ImuDriver::openDriver(void)
 {
   //setDriverId(driver string id);
+  this->lock();
   if(this->imu_device!=NULL)
   {
     delete this->imu_device;
@@ -20,6 +21,7 @@ bool Bno055ImuDriver::openDriver(void)
     this->imu_device=new CBNO055IMUDriver("BNO055_driver");
     this->imu_device->open(this->config_.serial_device);
     ROS_INFO("IMU successfully opened");
+    this->unlock();
     return true;
   }catch(CException &e){
     if(this->imu_device!=NULL)
@@ -28,33 +30,39 @@ bool Bno055ImuDriver::openDriver(void)
       this->imu_device=NULL;
     }
     ROS_WARN_STREAM(e.what());
+    this->unlock();
     return false;
   }
 }
 
 bool Bno055ImuDriver::closeDriver(void)
 {
+  this->lock();
   if(this->imu_device!=NULL)
   {
     delete this->imu_device;
     this->imu_device=NULL;
   }
+  this->unlock();
   return true;
 }
 
 bool Bno055ImuDriver::startDriver(void)
 {
+  this->lock();
   if(this->config_.cal_filename.size()>0)
   {
     try{
       this->imu_device->set_operation_mode(config_mode);
       this->imu_device->load_calibration(this->config_.cal_filename);
-      this->imu_device->set_operation_mode(ndof_mode);
+      this->imu_device->set_operation_mode((op_mode_t)this->config_.mode);
       this->imu_device->set_data_rate(this->config_.update_rate);
       ROS_INFO("IMU successfully started");
+      this->unlock();
       return true;
     }catch(CException &e){
       ROS_WARN_STREAM(e.what());
+      this->unlock();
       return false;
     }
   }
@@ -63,11 +71,13 @@ bool Bno055ImuDriver::startDriver(void)
     try{
       this->imu_device->set_operation_mode(config_mode);
       this->imu_device->set_data_rate(this->config_.update_rate);
-      this->imu_device->set_operation_mode(ndof_mode);
-      ROS_INFO("IMU successfully stopped");
+      this->imu_device->set_operation_mode((op_mode_t)this->config_.mode);
+      ROS_INFO("IMU successfully started");
+      this->unlock();
       return true;
     }catch(CException &e){
       ROS_WARN_STREAM(e.what());
+      this->unlock();
       return false;
     }
   }
@@ -75,10 +85,20 @@ bool Bno055ImuDriver::startDriver(void)
 
 bool Bno055ImuDriver::stopDriver(void)
 {
+  this->lock();
   this->imu_device->set_operation_mode(config_mode);
+  this->unlock();
   return true;
 }
 
+op_mode_t Bno055ImuDriver::get_operation_mode(void)
+{
+  if(this->imu_device!=NULL) 
+    return this->imu_device->get_operation_mode();
+  else
+    return config_mode;
+}
+
 void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level)
 {
   this->lock();
@@ -144,6 +164,30 @@ std::vector<double> Bno055ImuDriver::get_gravity(void)
     return std::vector<double>();
 }
 
+std::vector<double> Bno055ImuDriver::get_raw_accelerometer(void)
+{
+  if(this->imu_device!=NULL)
+    return this->imu_device->get_raw_accelerometer();
+  else
+    return std::vector<double>();
+}
+
+std::vector<double> Bno055ImuDriver::get_raw_magnetometer(void)
+{
+  if(this->imu_device!=NULL)
+    return this->imu_device->get_raw_magnetometer();
+  else
+    return std::vector<double>();
+}
+
+std::vector<double> Bno055ImuDriver::get_raw_gyroscope(void)
+{
+  if(this->imu_device!=NULL)
+    return this->imu_device->get_raw_gyroscope();
+  else
+    return std::vector<double>();
+}
+
 bool Bno055ImuDriver::is_imu_calibrated(void)
 {
   bool accel_cal,mag_cal,gyro_cal;

src/bno055_imu_ros_driver_node.cpp

@@ -10,7 +10,8 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) :
   this->setRate(500);//in [Hz]
 
   // [init publishers]
-  this->imu_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Imu>("imu", 1);
+  this->magnetometer_publisher_ = this->private_node_handle_.advertise<sensor_msgs::MagneticField>("magnetometer", 1);
+  this->imu_publisher_ = this->private_node_handle_.advertise<sensor_msgs::Imu>("imu", 1);
   
   // [init subscribers]
   
@@ -29,39 +30,67 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) :
     this->imu_Imu_msg_.orientation_covariance[i]=0.0;
     this->imu_Imu_msg_.angular_velocity_covariance[i]=0.0;
     this->imu_Imu_msg_.linear_acceleration_covariance[i]=0.0;
+    this->magnetometer_MagneticField_msg_.magnetic_field_covariance[i]=0.0;
   }
-  this->imu_Imu_msg_.angular_velocity_covariance[0]=-1.0;
 }
 
 void Bno055ImuDriverNode::mainNodeThread(void)
 {
-  std::vector<double> quaternion,linear_accel;
+  std::vector<double> quaternion,linear_accel,mag,gyro,accel;
+  op_mode_t op_mode;
 
   //lock access to driver if necessary
-  this->driver_.lock();
 
-  if(this->driver_.is_imu_calibrated())
+  this->driver_.lock();
+  if(this->driver_.getState()==iri_base_driver::RUNNING)
   {
+    this->driver_.is_imu_calibrated();
     try{
       this->events.clear();
       this->events.push_back(this->driver_.get_new_data_event_id());
       this->event_server->wait_all(events,2000);
+      op_mode=this->driver_.get_operation_mode();
+      if(op_mode==mag_only_mode || op_mode==acc_mag_mode || op_mode==mag_gyro_mode || op_mode==acc_mag_gyro_mode || op_mode==compass_mode || op_mode==m4g_mode || op_mode==ndof_off_mode || op_mode==ndof_mode)
+      {
+        mag=this->driver_.get_raw_magnetometer();
+        this->magnetometer_MagneticField_msg_.header.stamp=ros::Time::now();
+        this->magnetometer_MagneticField_msg_.header.frame_id=this->driver_.get_frame_id();
+        this->magnetometer_MagneticField_msg_.magnetic_field.x=mag[0];
+        this->magnetometer_MagneticField_msg_.magnetic_field.y=mag[1];
+        this->magnetometer_MagneticField_msg_.magnetic_field.z=mag[2];
+        this->magnetometer_publisher_.publish(this->magnetometer_MagneticField_msg_);
+      }
       quaternion=this->driver_.get_orientation_quat();
       linear_accel=this->driver_.get_linear_accel();
+      accel=this->driver_.get_raw_accelerometer();
+      gyro=this->driver_.get_raw_gyroscope();
       this->imu_Imu_msg_.header.stamp=ros::Time::now();
       this->imu_Imu_msg_.header.frame_id=this->driver_.get_frame_id();
       this->imu_Imu_msg_.orientation.x=quaternion[0];
       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_.linear_acceleration.x=linear_accel[0];
-      this->imu_Imu_msg_.linear_acceleration.y=linear_accel[1];
-      this->imu_Imu_msg_.linear_acceleration.z=linear_accel[2];
+      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];
+      if(op_mode>=imu_mode)
+      {
+        this->imu_Imu_msg_.linear_acceleration.x=linear_accel[0];
+        this->imu_Imu_msg_.linear_acceleration.y=linear_accel[1];
+        this->imu_Imu_msg_.linear_acceleration.z=linear_accel[2];
+      }
+      else
+      {
+        this->imu_Imu_msg_.linear_acceleration.x=accel[0];
+        this->imu_Imu_msg_.linear_acceleration.y=accel[1];
+        this->imu_Imu_msg_.linear_acceleration.z=accel[2];
+      }
       this->imu_publisher_.publish(this->imu_Imu_msg_);
     }catch(CException &e){  
       ROS_WARN_STREAM(e.what());
     }
   }
+  this->driver_.unlock();
 
   // [fill msg Header if necessary]
 
@@ -74,7 +103,6 @@ void Bno055ImuDriverNode::mainNodeThread(void)
   // [publish messages]
 
   //unlock access to driver if previously blocked
-  this->driver_.unlock();
 }
 
 /*  [subscriber callbacks] */