diff --git a/CMakeLists.txt b/CMakeLists.txt
index 25e69acf93761127354217899ff4b7ef7dc818c5..e1e039a8534f25fe45b6ad2d91b5324042316cbd 100644
--- a/CMakeLists.txt
+++ b/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})
 
diff --git a/calibration/bno055.cal b/calibration/bno055.cal
new file mode 100644
index 0000000000000000000000000000000000000000..415824d33b03a35d9afda9fda66eb71dd64af48e
Binary files /dev/null and b/calibration/bno055.cal differ
diff --git a/cfg/Bno055Imu.cfg b/cfg/Bno055Imu.cfg
index 1f607522a12e45a0e042a09e48d71e0821ee15a5..5a4c162257aafa1cec0870d650848b066996cb16 100755
--- a/cfg/Bno055Imu.cfg
+++ b/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"))
diff --git a/include/bno055_imu_ros_driver.h b/include/bno055_imu_ros_driver.h
index 4aa997334108e744f56ad13de03990170b6c5205..934d6e2973852b6e81062bebae2c52b320c4fcc1 100644
--- a/include/bno055_imu_ros_driver.h
+++ b/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
     *
diff --git a/include/bno055_imu_ros_driver_node.h b/include/bno055_imu_ros_driver_node.h
index 2320ff3fa59ef8561df2c9621fcda40d5f82ecd2..66ab1b0d47ab78da580f6b82f805c3a5408c283d 100644
--- a/include/bno055_imu_ros_driver_node.h
+++ b/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
diff --git a/launch/bno055_imu.launch b/launch/bno055_imu.launch
new file mode 100644
index 0000000000000000000000000000000000000000..b9d85ad4cc5a5ad9a6aa05e98872917b5a05f504
--- /dev/null
+++ b/launch/bno055_imu.launch
@@ -0,0 +1,16 @@
+<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>
+
diff --git a/src/bno055_imu_ros_driver.cpp b/src/bno055_imu_ros_driver.cpp
index 6fce11e22ad0a728d1c66ccca067e4e76c180e68..07d0a50f7fcfafd4a27555b6b162653124c15aec 100644
--- a/src/bno055_imu_ros_driver.cpp
+++ b/src/bno055_imu_ros_driver.cpp
@@ -1,29 +1,78 @@
 #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)
 {
 }
diff --git a/src/bno055_imu_ros_driver_node.cpp b/src/bno055_imu_ros_driver_node.cpp
index ff4461aec26933b5a72e0829ef8db78a74c15e15..de25be4a5d380077dc33fd3d73ca4f0bbfa5bd82 100644
--- a/src/bno055_imu_ros_driver_node.cpp
+++ b/src/bno055_imu_ros_driver_node.cpp
@@ -1,10 +1,13 @@
 #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();