Added the iriutils dependency.

added the tf_prefix and frame_id parameters to the dynamic reconfigure.
Implemented the driver functions.

CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS iri_base_driver sensor_msgs)
 # ******************************************************************** 
 #           Add system and labrobotica dependencies here
 # ******************************************************************** 
+find_package(iriutils REQUIRED)
 find_package(bno055_imu_driver REQUIRED)
 # find_package(<dependency> REQUIRED)
 
@@ -77,6 +78,7 @@ catkin_package(
 # ******************************************************************** 
 include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIR})
 include_directories(${bno055_imu_driver_INCLUDE_DIR})
 # include_directories(${<dependency>_INCLUDE_DIR})
 
@@ -90,6 +92,7 @@ add_executable(${PROJECT_NAME} src/bno055_imu_ros_driver.cpp src/bno055_imu_ros_
 #                   Add the libraries
 # ******************************************************************** 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
 target_link_libraries(${PROJECT_NAME} ${bno055_imu_driver_LIBRARY})
 # target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
 

cfg/Bno055Imu.cfg

@@ -42,6 +42,8 @@ gen = ParameterGenerator()
 gen.add("update_rate",             double_t,  SensorLevels.RECONFIGURE_STOP,   "Data update rate in Hz",         20.0,     1.0,  100.0)
 gen.add("serial_device",           str_t,     SensorLevels.RECONFIGURE_STOP,   "Device serial port",             "/dev/ttyUSB0")
 gen.add("cal_filename",            str_t,     SensorLevels.RECONFIGURE_STOP,   "Sensor calibration data",        "")
+gen.add("tf_prefix",               str_t,     SensorLevels.RECONFIGURE_STOP,   "TF prefix",                      "")
+gen.add("frame_id",                str_t,     SensorLevels.RECONFIGURE_STOP,   "IMU frame_id in the urdf file",  "")
 #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

@@ -54,7 +54,9 @@ class Bno055ImuDriver : public iri_base_driver::IriBaseDriver
 {
   private:
     // private attributes and methods
-    BNO055IMUDriver *imu_device;
+    CBNO055IMUDriver *imu_device;
+    std::string frame_id;
+    bool calibrated;
   public:
    /**
     * \brief define config type
@@ -150,7 +152,8 @@ class Bno055ImuDriver : public iri_base_driver::IriBaseDriver
     std::vector<double> get_orientation_euler(void);
     std::vector<double> get_linear_accel(void);
     std::vector<double> get_gravity(void);
-
+    bool is_imu_calibrated(void);
+    std::string get_frame_id(void);
    /**
     * \brief Destructor
     *

include/bno055_imu_ros_driver_node.h

@@ -35,6 +35,8 @@
 
 // [action server client headers]
 
+#include "eventserver.h"
+
 /**
  * \brief IRI ROS Specific Driver Class
  *
@@ -59,7 +61,6 @@ class Bno055ImuDriverNode : public iri_base_driver::IriBaseNodeDriver<Bno055ImuD
     ros::Publisher imu_publisher_;
     sensor_msgs::Imu imu_Imu_msg_;
 
-
     // [subscriber attributes]
 
     // [service attributes]
@@ -69,6 +70,9 @@ class Bno055ImuDriverNode : public iri_base_driver::IriBaseNodeDriver<Bno055ImuD
     // [action server attributes]
 
     // [action client attributes]
+   
+    std::list<std::string> events;
+    CEventServer *event_server;
 
    /**
     * \brief post open hook

launch/bno055_imu.launch

+<launch>
+
+  <!-- launch the action client node -->
+  <node name="bno055_imu"
+        pkg="bno055_imu"
+        type="bno055_imu"
+        output="screen">
+    <param name="update_rate" value="1"/>
+    <param name="serial_device" value="/dev/ttyUSB1"/>
+    <param name="cal_filename" value="$(find bno055_imu)/calibration/bno055.cal"/>
+    <param name="tf_prefix" value=""/>
+    <param name="frame_id" value="imu"/>
+  </node>
+
+</launch>
+

src/bno055_imu_ros_driver.cpp

 #include "bno055_imu_ros_driver.h"
+#include "exceptions.h"
 
 Bno055ImuDriver::Bno055ImuDriver(void)
 {
   //setDriverId(driver string id);
+  this->imu_device=NULL;
+  this->calibrated=false;
 }
 
 bool Bno055ImuDriver::openDriver(void)
 {
   //setDriverId(driver string id);
-
-  return true;
+  if(this->imu_device!=NULL)
+  {
+    delete this->imu_device;
+    this->imu_device=NULL;
+  }
+  try{
+    this->imu_device=new CBNO055IMUDriver("BNO055_driver");
+    this->imu_device->open(this->config_.serial_device);
+    return true;
+  }catch(CException &e){
+    if(this->imu_device!=NULL)
+    {
+      delete this->imu_device;
+      this->imu_device=NULL;
+    }
+    ROS_WARN_STREAM(e.what());
+    return false;
+  }
 }
 
 bool Bno055ImuDriver::closeDriver(void)
 {
+  if(this->imu_device!=NULL)
+  {
+    delete this->imu_device;
+    this->imu_device=NULL;
+  }
   return true;
 }
 
 bool Bno055ImuDriver::startDriver(void)
 {
-  return true;
+  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_data_rate(this->config_.update_rate);
+      return true;
+    }catch(CException &e){
+      ROS_WARN_STREAM(e.what());
+      return false;
+    }
+  }
+  else
+  {
+    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);
+      return true;
+    }catch(CException &e){
+      ROS_WARN_STREAM(e.what());
+      return false;
+    }
+  }
 }
 
 bool Bno055ImuDriver::stopDriver(void)
 {
+  this->imu_device->set_operation_mode(config_mode);
   return true;
 }
 
@@ -36,6 +85,7 @@ void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level)
   switch(this->getState())
   {
     case Bno055ImuDriver::CLOSED:
+      this->frame_id=new_cfg.tf_prefix+"/"+new_cfg.frame_id;
       break;
 
     case Bno055ImuDriver::OPENED:
@@ -51,6 +101,85 @@ void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level)
   this->unlock();
 }
 
+std::string Bno055ImuDriver::get_new_data_event_id(void)
+{
+  if(this->imu_device!=NULL)
+    return this->imu_device->get_new_data_event_id();
+  else
+    return std::string("");
+}
+
+std::vector<double> Bno055ImuDriver::get_orientation_quat(void)
+{
+  if(this->imu_device!=NULL)
+    return this->imu_device->get_orientation_quat();
+  else
+    return std::vector<double>();
+}
+
+std::vector<double> Bno055ImuDriver::get_orientation_euler(void)
+{
+  if(this->imu_device!=NULL)
+    return this->imu_device->get_orientation_euler();
+  else
+    return std::vector<double>();
+}
+
+std::vector<double> Bno055ImuDriver::get_linear_accel(void)
+{
+  if(this->imu_device!=NULL)
+    return this->imu_device->get_linear_accel();
+  else
+    return std::vector<double>();
+}
+
+std::vector<double> Bno055ImuDriver::get_gravity(void)
+{
+  if(this->imu_device!=NULL)
+    return this->imu_device->get_gravity();
+  else
+    return std::vector<double>();
+}
+
+bool Bno055ImuDriver::is_imu_calibrated(void)
+{
+  bool accel_cal,mag_cal,gyro_cal;
+
+  if(!this->calibrated)
+  {
+    try{
+      accel_cal=this->imu_device->is_accelerometer_calibrated();
+      mag_cal=this->imu_device->is_magnetometer_calibrated();
+      gyro_cal=this->imu_device->is_gyroscope_calibrated();
+      if(accel_cal && mag_cal && gyro_cal)
+      {
+        this->calibrated=true;
+        return true;
+      }
+      else
+      {
+        if(!accel_cal)
+          ROS_WARN("Accelerometer not calibrated: Move the sensor slowly to six different positions and hold it steady for a few seconds");
+        if(!mag_cal)
+          ROS_WARN("Magnetometer no calibrated: Move the sensor randomly");
+        if(!gyro_cal)
+          ROS_WARN("Gyroscope not calibrated: Hold the sensor steady at any position for a few seconds");
+        return false;
+      }
+    }catch(CException &e){
+      ROS_WARN_STREAM(e.what());
+      return false;
+    }
+  }
+  else
+    return this->calibrated;
+}
+
+std::string Bno055ImuDriver::get_frame_id(void)
+{
+  return this->frame_id;
+}
+
 Bno055ImuDriver::~Bno055ImuDriver(void)
 {
 }

src/bno055_imu_ros_driver_node.cpp

 #include "bno055_imu_ros_driver_node.h"
+#include "exceptions.h"
 
 Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) : 
   iri_base_driver::IriBaseNodeDriver<Bno055ImuDriver>(nh)
 {
+  unsigned int i;
+
   //init class attributes if necessary
-  //this->loop_rate_ = 2;//in [Hz]
+  this->loop_rate_ = 500;//in [Hz]
 
   // [init publishers]
   this->imu_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Imu>("imu", 1);
@@ -18,28 +21,57 @@ Bno055ImuDriverNode::Bno055ImuDriverNode(ros::NodeHandle &nh) :
   // [init action servers]
   
   // [init action clients]
+
+  this->event_server=CEventServer::instance();
+  // initialize message
+  for(i=0;i<9;i++)
+  {
+    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->imu_Imu_msg_.angular_velocity_covariance[0]=-1.0;
 }
 
 void Bno055ImuDriverNode::mainNodeThread(void)
 {
+  std::vector<double> quaternion,linear_accel;
+
   //lock access to driver if necessary
   this->driver_.lock();
 
+  if(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);
+      quaternion=this->driver_.get_orientation_quat();
+      linear_accel=this->driver_.get_linear_accel();
+      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_publisher_.publish(this->imu_Imu_msg_);
+    }catch(CException &e){  
+      ROS_WARN_STREAM(e.what());
+    }
+  }
+
   // [fill msg Header if necessary]
 
   // [fill msg structures]
-  // Initialize the topic message structure
-  //this->imu_Imu_msg_.data = my_var;
-
   
   // [fill srv structure and make request to the server]
   
   // [fill action structure and make request to the action server]
 
   // [publish messages]
-  // Uncomment the following line to publish the topic message
-  //this->imu_publisher_.publish(this->imu_Imu_msg_);
-
 
   //unlock access to driver if previously blocked
   this->driver_.unlock();