diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7624e98dc2fb7255cd07f562a3622d80067587b6..45718d7b84b8af4dd840ab7e62de65eb7f776cdd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,10 +6,12 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_driver)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver sensor_msgs image_transport camera_info_manager)
+# find_package(catkin REQUIRED COMPONENTS iri_base_driver cv_bridge image_transport camera_info_manager sensor_msgs)
 find_package(mvbluefox3 REQUIRED)
 find_package(mvIMPACT REQUIRED)
-# FIND_PACKAGE(OpenCV 3)
+FIND_PACKAGE(OpenCV 3 REQUIRED)
+# find_package(OpenCV 2.4.8 REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -63,12 +65,13 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_driver
+ CATKIN_DEPENDS iri_base_driver sensor_msgs image_transport camera_info_manager
+ # CATKIN_DEPENDS iri_base_driver cv_bridge image_transport camera_info_manager sensor_msgs
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
- DEPENDS mvbluefox3 mvIMPACT
- # DEPENDS mvbluefox3 mvIMPACT OpenCV
+ # DEPENDS mvbluefox3 mvIMPACT
+ DEPENDS mvbluefox3 mvIMPACT OpenCV
 )
 
 ###########
@@ -82,7 +85,7 @@ include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
 include_directories(${mvbluefox3_INCLUDE_DIR})
 include_directories(${mvIMPACT_INCLUDE_DIR})
-# INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
 # include_directories(${<dependency>_INCLUDE_DIR})
 
 ## Declare a cpp library
@@ -97,6 +100,7 @@ add_executable(${PROJECT_NAME} src/mvbluefox3_camera_driver.cpp src/mvbluefox3_c
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 target_link_libraries(${PROJECT_NAME} ${mvbluefox3_LIBRARY})
 target_link_libraries(${PROJECT_NAME} ${mvIMPACT_LIBRARY})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBRARIES})
 # TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS})
 # target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
 
@@ -104,6 +108,10 @@ target_link_libraries(${PROJECT_NAME} ${mvIMPACT_LIBRARY})
 #               Add message headers dependencies 
 # ******************************************************************** 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} cv_bridge_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} image_transport_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} camera_info_manager_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
 # ******************************************************************** 
 #               Add dynamic reconfigure dependencies 
 # ******************************************************************** 
diff --git a/include/mvbluefox3_camera_driver.h b/include/mvbluefox3_camera_driver.h
index 7465a462c4e160733a546867f65f6b6165ce8b2d..d2432c7f33516aae2d2705de7c4d553f8f51facf 100644
--- a/include/mvbluefox3_camera_driver.h
+++ b/include/mvbluefox3_camera_driver.h
@@ -30,6 +30,9 @@
 
 #include "mvbluefox3.h"
 
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/fill_image.h>
+
 /**
  * \brief IRI ROS Specific Driver Class
  *
@@ -177,33 +180,13 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
     */
     void config_update(Config& new_cfg, uint32_t level=0);
 
-   /**
-    * \brief Get image channel's depth
-    *
-    * Get image channel's depth.
-    */  
-    int GetChDepth(void);
-
-   /**
-    * \brief Get bytes-pixel
-    *
-    * Get bytes-pixel.
-    */ 
-    int GetBytesXPixel(void);
-
    /**
     * \brief Get image
     *
     * Get image.
     */ 
-    void GetImage(char *image);
-
-   /**
-    * \brief Get real exposure time
-    *
-    * Get real exposure time.
-    */ 
-    int ReqExposeUs(void);
+    // void GetImage(char *image);
+    void GetROSImage(std::string &frame_id, sensor_msgs::Image &img_msg);
 
    /**
     * \brief Destructor
diff --git a/include/mvbluefox3_camera_driver_node.h b/include/mvbluefox3_camera_driver_node.h
index 4ecdfdb79a2151d0e7fe249732582e63331d06b2..30756e83677d619dae98c6c5f0a168d6d757599f 100644
--- a/include/mvbluefox3_camera_driver_node.h
+++ b/include/mvbluefox3_camera_driver_node.h
@@ -29,6 +29,11 @@
 #include "mvbluefox3_camera_driver.h"
 
 // [publisher subscriber headers]
+#include <camera_info_manager/camera_info_manager.h>
+#include <image_transport/image_transport.h>
+// // Uncomment to use the openCV <-> ROS bridge
+// //#include <cv_bridge/cv_bridge.h>
+// #include <sensor_msgs/Image.h>
 
 // [service client headers]
 
@@ -57,6 +62,15 @@ class Mvbluefox3CameraDriverNode : public iri_base_driver::IriBaseNodeDriver<Mvb
 
     // [publisher attributes]
 
+    // Uncomment to use the openCV <-> ROS bridge
+    //cv_bridge::CvImagePtr cv_image_;
+    image_transport::ImageTransport it;
+
+    camera_info_manager::CameraInfoManager camera_manager;
+    image_transport::CameraPublisher image_publisher_;
+    sensor_msgs::Image image_msg_;
+
+
     // [subscriber attributes]
 
     // [service attributes]
diff --git a/package.xml b/package.xml
index 4bfa0e419b084b0d4f6affd32c2315158238f393..236b83fefaa1ef446f67298ee2e3982813f3a6bf 100644
--- a/package.xml
+++ b/package.xml
@@ -41,7 +41,15 @@
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>iri_base_driver</build_depend>
+  <!-- <build_depend>cv_bridge</build_depend> -->
+  <build_depend>image_transport</build_depend>
+  <build_depend>camera_info_manager</build_depend>
+  <build_depend>sensor_msgs</build_depend>
   <run_depend>iri_base_driver</run_depend>
+  <!-- <run_depend>cv_bridge</run_depend> -->
+  <run_depend>image_transport</run_depend>
+  <run_depend>camera_info_manager</run_depend>
+  <run_depend>sensor_msgs</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/src/mvbluefox3_camera_driver.cpp b/src/mvbluefox3_camera_driver.cpp
index a3080a431ded64a733eed5c796343a6ec3503c56..c45a07318b36ad710bf51fc1f42a029c70a2666a 100644
--- a/src/mvbluefox3_camera_driver.cpp
+++ b/src/mvbluefox3_camera_driver.cpp
@@ -1,14 +1,7 @@
 #include "mvbluefox3_camera_driver.h"
 
-// #include <opencv2/opencv.hpp>
-// #define WINDOW_NAME "mvBlueFOX3 TEST"
-
 Mvbluefox3CameraDriver::Mvbluefox3CameraDriver(void)
 {
-  this->cam_ptr = NULL;
-
-  // TMP: To be substituted by config_update
-  this->serial = "F0300141";
 }
 
 Mvbluefox3CameraDriver::~Mvbluefox3CameraDriver(void)
@@ -24,32 +17,19 @@ bool Mvbluefox3CameraDriver::openDriver(void)
   }catch (CMvbluefox3::CmvBlueFOX3Exception& e) 
   {
     std::cout << e.what() << std::endl;
-  }    
+  }   
   return true;
 }
 
 bool Mvbluefox3CameraDriver::closeDriver(void)
 {
+  this->cam_ptr->CloseDevice();
   return true;
 }
 
 bool Mvbluefox3CameraDriver::startDriver(void)
 {
-  // std::cout << "[Camera test]: Acquiring images." << std::endl;
-
-  // cv::namedWindow( WINDOW_NAME, cv::WINDOW_NORMAL );
-    
-  // while (true) 
-  // {
-  //   cv::Mat image;
-  //   this->cam_ptr->GetImageCV(image);
-  //   cv::imshow( WINDOW_NAME, image );
-  //   if(cv::waitKey(30) >= 0) break;
-  // }
-  // cv::destroyAllWindows();
-
   std::cout << "[mvBlueFOX3]: Driver started." << std::endl;
-
   return true;
 }
 
@@ -67,6 +47,8 @@ void Mvbluefox3CameraDriver::config_update(Config& new_cfg, uint32_t level)
   switch(this->getState())
   {
     case Mvbluefox3CameraDriver::CLOSED: 
+      // TMP: To be substituted by config_update
+      this->serial = "F0300141";
       break;
 
     case Mvbluefox3CameraDriver::OPENED:
@@ -108,23 +90,27 @@ void Mvbluefox3CameraDriver::GetConfig(void)
   this->params = this->cam_ptr->GetConfig();
 }
 
-int Mvbluefox3CameraDriver::GetChDepth(void)
+void Mvbluefox3CameraDriver::GetROSImage(std::string &frame_id, sensor_msgs::Image &img_msg)
 {
-  return this->cam_ptr->GetChDepth();
-}
+  ros::Time start_time = ros::Time::now();
 
-int Mvbluefox3CameraDriver::GetBytesXPixel(void)
-{
-  return this->cam_ptr->GetBytesXPixel();
-}
+  double numbytes = (double)(this->cam_ptr->GetChDepth() * this->cam_ptr->GetBytesXPixel());
 
-void Mvbluefox3CameraDriver::GetImage(char *image)
-{
+  char *image;
   this->cam_ptr->GetImage(&image);
-}
 
-int Mvbluefox3CameraDriver::ReqExposeUs(void)
-{
-  return this->cam_ptr->ReqExposeUs();
+  sensor_msgs::fillImage(img_msg, 
+    CMvbluefox3::codings_str[this->params.pixel_format], 
+    this->params.height,
+    this->params.width,
+    this->params.width*numbytes/8.0,
+    image);
+
+  double req_dur_sec = (double)(this->cam_ptr->ReqExposeUs())*1e-6;
+  img_msg.header.stamp = start_time + ros::Duration(req_dur_sec/2.0);
+  img_msg.header.frame_id = frame_id;
+
+  delete [] image;
 }
 
+
diff --git a/src/mvbluefox3_camera_driver_node.cpp b/src/mvbluefox3_camera_driver_node.cpp
index c5ab654f37813335d0cdeafe34d8c78f1e2adaf2..44b0e4fa705af0884e8d57a2c518edc4ea84b4a8 100644
--- a/src/mvbluefox3_camera_driver_node.cpp
+++ b/src/mvbluefox3_camera_driver_node.cpp
@@ -1,12 +1,25 @@
 #include "mvbluefox3_camera_driver_node.h"
 
 Mvbluefox3CameraDriverNode::Mvbluefox3CameraDriverNode(ros::NodeHandle &nh) : 
-  iri_base_driver::IriBaseNodeDriver<Mvbluefox3CameraDriver>(nh)
+  iri_base_driver::IriBaseNodeDriver<Mvbluefox3CameraDriver>(nh),
+  camera_manager(ros::NodeHandle("~image_raw")),
+  it(this->public_node_handle_)
 {
   //init class attributes if necessary
   this->loop_rate_ = 50;//in [Hz]
 
   // [init publishers]
+  this->image_publisher_ = this->it.advertiseCamera("image_raw", 1);
+
+  std::string image_cal_file;
+  public_node_handle_.param<std::string>("calibration_file",image_cal_file,"");
+  if(this->camera_manager.validateURL(image_cal_file))
+  {
+   if(!this->camera_manager.loadCameraInfo(image_cal_file))
+     ROS_INFO("Invalid calibration file");
+  }
+  else
+   ROS_INFO("Invalid calibration file");
   
   // [init subscribers]
   
@@ -21,19 +34,29 @@ Mvbluefox3CameraDriverNode::Mvbluefox3CameraDriverNode(ros::NodeHandle &nh) :
 
 void Mvbluefox3CameraDriverNode::mainNodeThread(void)
 {
-  this->driver_.lock();
-
   // [fill msg Header if necessary]
 
   // [fill msg structures]
+
+  // Image
+  this->driver_.lock();
+  std::string frame_id = "mvBlueFOX3";
+  this->driver_.GetROSImage(frame_id,this->image_msg_);
+  this->driver_.unlock();
+
+  // Camera info
+  sensor_msgs::CameraInfo camera_info = this->camera_manager.getCameraInfo();
+  camera_info.header = this->image_msg_.header;
   
   // [fill srv structure and make request to the server]
   
   // [fill action structure and make request to the action server]
 
   // [publish messages]
-
-  this->driver_.unlock();
+  // Uncomment the following line to convert an OpenCV image to a ROS image message
+  //this->image_msg_=*this->cv_image_->toImageMsg();
+  // Uncomment the following line to publish the image together with the camera information
+  this->image_publisher_.publish(this->image_msg_,camera_info);
 }
 
 /*  [subscriber callbacks] */