From 3665eaadcfe15d833be5c441ece57acd80543be5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Tue, 10 Mar 2015 11:11:42 +0000
Subject: [PATCH] Removed the nodelet module. Now the node and nodelet use the
 same code. Removed the calibration file path from the dynamic reconfigure.
 The parameter still exists with the same name, but it can not be modified.
 Added check to verify that the current configuration coincides with the
 calibration configuration.

---
 CMakeLists.txt                           |   2 +-
 cfg/FirewireCamera.cfg                   |   1 -
 include/camera_common.h                  |   2 +-
 include/firewire_camera_driver.h         |  12 +-
 include/firewire_camera_driver_node.h    |  30 ++-
 include/firewire_camera_driver_nodelet.h | 222 --------------------
 src/firewire_camera_driver.cpp           |  33 +--
 src/firewire_camera_driver_node.cpp      | 127 ++++++++----
 src/firewire_camera_driver_nodelet.cpp   | 246 -----------------------
 9 files changed, 143 insertions(+), 532 deletions(-)
 delete mode 100755 include/firewire_camera_driver_nodelet.h
 delete mode 100755 src/firewire_camera_driver_nodelet.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index aa1ad1c..53f5468 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -95,7 +95,7 @@ include_directories(${raw1394_INCLUDE_DIR})
 include_directories(${dc1394_INCLUDE_DIR})
 
 ## Declare a cpp library
-add_library(${PROJECT_NAME}_nodelet src/firewire_camera_driver_nodelet.cpp src/firewire_camera_driver.cpp)
+add_library(${PROJECT_NAME}_nodelet src/firewire_camera_driver_node.cpp src/firewire_camera_driver.cpp)
 # add_library(${PROJECT_NAME} <list of source files>)
 
 ## Declare a cpp executable
diff --git a/cfg/FirewireCamera.cfg b/cfg/FirewireCamera.cfg
index 9eaebda..7a78023 100755
--- a/cfg/FirewireCamera.cfg
+++ b/cfg/FirewireCamera.cfg
@@ -54,7 +54,6 @@ gen.const("RAW16", int_t, 7, "RAW coding with 16 bits per pixel")],"Available co
 gen.add("frame_id",                str_t,     SensorLevels.RECONFIGURE_STOP,   "Camera frame identifier",        "camera_frame")
 gen.add("Camera_node",             int_t,     SensorLevels.RECONFIGURE_STOP,   "Desired camera id",              -1,       -1 ,  100)
 gen.add("Camera_serial",           str_t,     SensorLevels.RECONFIGURE_STOP,   "Desired camera serial number",   "")
-gen.add("cal_file",                str_t,     SensorLevels.RECONFIGURE_STOP,   "Camera calibration file",        "")
 gen.add("ISO_speed",               int_t,     SensorLevels.RECONFIGURE_STOP,   "Desired ISO speed",              800,      100,  800)
 gen.add("Image_width",             int_t,     SensorLevels.RECONFIGURE_STOP,   "Desired image width in pixels",  640,      160 , 2448)
 gen.add("Image_height",            int_t,     SensorLevels.RECONFIGURE_STOP,   "Desired image height in pixels", 480,      120 , 2048)
diff --git a/include/camera_common.h b/include/camera_common.h
index 39264b0..26415df 100755
--- a/include/camera_common.h
+++ b/include/camera_common.h
@@ -29,7 +29,7 @@ typedef struct
 
 typedef struct
 {
-  image_transport::ImageTransport *it;
+  boost::shared_ptr<image_transport::ImageTransport> it;
   image_transport::CameraPublisher camera_image_publisher_;
   sensor_msgs::ImagePtr Image_msg_;
   sensor_msgs::CameraInfoPtr CameraInfo_msg_;
diff --git a/include/firewire_camera_driver.h b/include/firewire_camera_driver.h
index 4b39431..585e111 100644
--- a/include/firewire_camera_driver.h
+++ b/include/firewire_camera_driver.h
@@ -67,10 +67,10 @@ class FirewireCameraDriver : public iri_base_driver::IriBaseDriver
     TCameraConfig current_conf;
     // default_configuration
     TCameraConfig default_conf;
-    // calibration file
-    std::string calibration_file;
     // frame identifier
     std::string frame_id;
+    unsigned int cal_width;
+    unsigned int cal_height;
   protected:
     /**
      * \brief
@@ -201,7 +201,7 @@ class FirewireCameraDriver : public iri_base_driver::IriBaseDriver
      * \brief
      *
      */ 
-    void get_image(char **image_data);
+    void get_image(char *image_data);
     /**
      * \brief
      *
@@ -237,6 +237,12 @@ class FirewireCameraDriver : public iri_base_driver::IriBaseDriver
      *
      */
     std::string get_frame_id(void);
+    /**
+     * \brief
+     *
+     */
+    void set_calibration_params(unsigned int width, unsigned int height);
+
    /**
     * \brief Destructor
     *
diff --git a/include/firewire_camera_driver_node.h b/include/firewire_camera_driver_node.h
index c3b799d..976ce60 100644
--- a/include/firewire_camera_driver_node.h
+++ b/include/firewire_camera_driver_node.h
@@ -44,6 +44,7 @@
 // [action server client headers]
 
 #include "eventserver.h"
+#include "camera_common.h"
 
 /**
  * \brief IRI ROS Specific Driver Class
@@ -66,9 +67,7 @@ class FirewireCameraDriverNode : public iri_base_driver::IriBaseNodeDriver<Firew
 {
   private:
     // [publisher attributes]
-    image_transport::ImageTransport *it;
-    image_transport::CameraPublisher camera_image_publisher_;
-    sensor_msgs::Image Image_msg_;
+    TROSCameraPtr camera;
     camera_info_manager::CameraInfoManager camera_manager;
     diagnostic_updater::HeaderlessTopicDiagnostic *diagnosed_camera_image;
 
@@ -201,4 +200,29 @@ class FirewireCameraDriverNode : public iri_base_driver::IriBaseNodeDriver<Firew
 
 };
 
+#include "nodelet/nodelet.h"
+
+class FirewireCameraNodelet : public nodelet::Nodelet
+{
+  private:
+    FirewireCameraDriverNode *node;
+    virtual void onInit();// initialization function
+    // thread attributes
+    CThreadServer *thread_server;
+    std::string spin_thread_id;
+  protected:
+    static void *spin_thread(void *param);
+  public:
+    FirewireCameraNodelet();
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~FirewireCameraNodelet();
+};
+
+
 #endif
diff --git a/include/firewire_camera_driver_nodelet.h b/include/firewire_camera_driver_nodelet.h
deleted file mode 100755
index c11228c..0000000
--- a/include/firewire_camera_driver_nodelet.h
+++ /dev/null
@@ -1,222 +0,0 @@
-// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
-// Author 
-// All rights reserved.
-//
-// This file is part of iri-ros-pkg
-// iri-ros-pkg is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-// 
-// IMPORTANT NOTE: This code has been generated through a script from the 
-// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
-// of the scripts. ROS topics can be easly add by using those scripts. Please
-// refer to the IRI wiki page for more information:
-// http://wikiri.upc.es/index.php/Robotics_Lab
-
-#ifndef _firewire_camera_driver_nodelet_h_
-#define _firewire_camera_driver_nodelet_h_
-
-#include "nodelet/nodelet.h"
-
-#include <iri_base_driver/iri_base_driver_node.h>
-#include "firewire_camera_driver.h"
-
-// diagnostic headers
-#include "diagnostic_updater/diagnostic_updater.h"
-#include "diagnostic_updater/publisher.h"
-
-// image transport
-#include "image_transport/image_transport.h"
-#include <camera_info_manager/camera_info_manager.h>
-
-// [publisher subscriber headers]
-#include <sensor_msgs/Image.h>
-
-// [service client headers]
-
-// [action server client headers]
-
-#include "eventserver.h"
-#include "threadserver.h"
-
-/**
- * \brief IRI ROS Specific Driver Class
- *
- * This class inherits from the IRI Core class IriBaseNodeDriver<IriBaseDriver>, 
- * to provide an execution thread to the driver object. A complete framework  
- * with utilites to test the node functionallity or to add diagnostics to  
- * specific situations is also given. The inherit template design form allows  
- * complete access to any IriBaseDriver object implementation.
- *
- * As mentioned, tests in the different driver states can be performed through 
- * class methods such as addNodeOpenedTests() or addNodeRunningTests(). Tests
- * common to all nodes may be also executed in the pattern class IriBaseNodeDriver.
- * Similarly to the tests, diagnostics can easyly be added. See ROS Wiki for
- * more details:
- * http://www.ros.org/wiki/diagnostics/ (Tutorials: Creating a Diagnostic Analyzer)
- * http://www.ros.org/wiki/self_test/ (Example: Self Test)
- */
-class FirewireCameraDriverNode : public iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>
-{
-  private:
-    // [publisher attributes]
-    camera_info_manager::CameraInfoManager camera_manager;
-    boost::shared_ptr<image_transport::ImageTransport> it;
-    sensor_msgs::ImagePtr Image_msg_;// this pointer is already a shared_ptr
-    image_transport::CameraPublisher camera_image_publisher_;
-    diagnostic_updater::HeaderlessTopicDiagnostic *diagnosed_camera_image;
-
-    // [subscriber attributes]
-
-    // [service attributes]
-
-    // [client attributes]
-
-    // [action server attributes]
-
-    // [action client attributes]
-
-    // node attributes
-    double desired_framerate;
-    CEventServer *event_server;
-    std::string new_frame_event;
-    std::string tf_prefix_;
-
-   /**
-    * \brief post open hook
-    * 
-    * This function is called by IriBaseNodeDriver::postOpenHook(). In this function
-    * specific parameters from the driver must be added so the ROS dynamic 
-    * reconfigure application can update them.
-    */
-    void postNodeOpenHook(void);
-
-  public:
-   /**
-    * \brief constructor
-    *
-    * This constructor mainly creates and initializes the FirewireCameraDriverNode topics
-    * through the given public_node_handle object. IriBaseNodeDriver attributes 
-    * may be also modified to suit node specifications.
-    *
-    * All kind of ROS topics (publishers, subscribers, servers or clients) can 
-    * be easyly generated with the scripts in the iri_ros_scripts package. Refer
-    * to ROS and IRI Wiki pages for more details:
-    *
-    * http://www.ros.org/wiki/ROS/Tutorials/WritingPublisherSubscriber(c++)
-    * http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient(c++)
-    * http://wikiri.upc.es/index.php/Robotics_Lab
-    *
-    * \param nh a reference to the node handle object to manage all ROS topics.
-    */
-    FirewireCameraDriverNode(ros::NodeHandle& nh);
-
-   /**
-    * \brief Destructor
-    *
-    * This destructor is called when the object is about to be destroyed.
-    *
-    */
-    ~FirewireCameraDriverNode();
-
-  protected:
-   /**
-    * \brief main node thread
-    *
-    * This is the main thread node function. Code written here will be executed
-    * in every node loop while the driver is on running state. Loop frequency 
-    * can be tuned my modifying loop_rate attribute.
-    *
-    * Here data related to the process loop or to ROS topics (mainly data structs
-    * related to the MSG and SRV files) must be updated. ROS publisher objects 
-    * must publish their data in this process. ROS client servers may also
-    * request data to the corresponding server topics.
-    */
-    void mainNodeThread(void);
-
-    // [diagnostic functions]
-    void check_configuration(diagnostic_updater::DiagnosticStatusWrapper &stat);
-
-   /**
-    * \brief node add diagnostics
-    *
-    * In this function ROS diagnostics applied to this specific node may be
-    * added. Common use diagnostics for all nodes are already called from 
-    * IriBaseNodeDriver::addDiagnostics(), which also calls this function. Information
-    * of how ROS diagnostics work can be readen here:
-    * http://www.ros.org/wiki/diagnostics/
-    * http://www.ros.org/doc/api/diagnostic_updater/html/example_8cpp-source.html
-    */
-    void addNodeDiagnostics(void);
-
-    // [driver test functions]
-
-   /**
-    * \brief open status driver tests
-    *
-    * In this function tests checking driver's functionallity when driver_base 
-    * status=open can be added. Common use tests for all nodes are already called
-    * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
-    * please refer to the Self Test example in:
-    * http://www.ros.org/wiki/self_test/
-    */
-    void addNodeOpenedTests(void);
-
-   /**
-    * \brief stop status driver tests
-    *
-    * In this function tests checking driver's functionallity when driver_base 
-    * status=stop can be added. Common use tests for all nodes are already called
-    * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
-    * please refer to the Self Test example in:
-    * http://www.ros.org/wiki/self_test/
-    */
-    void addNodeStoppedTests(void);
-
-   /**
-    * \brief run status driver tests
-    *
-    * In this function tests checking driver's functionallity when driver_base 
-    * status=run can be added. Common use tests for all nodes are already called
-    * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
-    * please refer to the Self Test example in:
-    * http://www.ros.org/wiki/self_test/
-    */
-    void addNodeRunningTests(void);
-
-   /**
-    * \brief specific node dynamic reconfigure
-    *
-    * This function is called reconfigureHook()
-    * 
-    * \param level integer
-    */
-    void reconfigureNodeHook(int level);
-
-};
-
-class FirewireCameraNodelet : public nodelet::Nodelet
-{
-  private:
-    virtual void onInit();// initialization function
-    FirewireCameraDriverNode *dev;
-    // thread attributes
-    CThreadServer *thread_server;
-    std::string spin_thread_id;
-  protected:
-    static void *spin_thread(void *param);
-  public:
-    FirewireCameraNodelet();
-    ~FirewireCameraNodelet();
-};
-
-#endif
diff --git a/src/firewire_camera_driver.cpp b/src/firewire_camera_driver.cpp
index 119860a..f315e77 100644
--- a/src/firewire_camera_driver.cpp
+++ b/src/firewire_camera_driver.cpp
@@ -32,10 +32,11 @@ FirewireCameraDriver::FirewireCameraDriver()
   this->default_conf.depth=(depths_t)-1;
   this->default_conf.coding=(codings_t)-1;
   this->default_conf.framerate=0.0;
-  // default calibration file
-  this->calibration_file="";
   // default frame id
   this->frame_id="";
+  // default calibration parameters
+  this->cal_width=-1;
+  this->cal_height=-1;
 }
 
 bool FirewireCameraDriver::openDriver(void)
@@ -61,6 +62,15 @@ bool FirewireCameraDriver::openDriver(void)
       this->camera->get_config(&this->default_conf.left_offset,&this->default_conf.top_offset,&this->default_conf.width,
                                &this->default_conf.height,&this->default_conf.framerate,&this->default_conf.depth,
                                &this->default_conf.coding);
+      // set the desired configuration
+      if((this->cal_width!=-1 && this->desired_conf.width!=this->cal_width) ||
+         (this->cal_height!=-1 && this->desired_conf.height!=this->cal_height))
+      {
+        ROS_ERROR("The calibration file does not coincide with the current configuration ");
+        delete this->camera;
+        this->camera=NULL;
+        return false;
+      }
       this->change_config(&this->desired_conf);
       ROS_INFO("Driver opened");
       return true;
@@ -144,8 +154,6 @@ void FirewireCameraDriver::config_update(Config& new_cfg, uint32_t level)
       this->camera_serial=new_cfg.Camera_serial;
       this->ISO_speed=new_cfg.ISO_speed;
       this->dyn_rec_to_camera(new_cfg,this->desired_conf);
-      // change the calibration file
-      this->calibration_file=new_cfg.cal_file;
       // update the frame identifier
       this->frame_id=new_cfg.frame_id;
       this->unlock();
@@ -181,8 +189,6 @@ void FirewireCameraDriver::config_update(Config& new_cfg, uint32_t level)
           this->set_shutter(&new_cfg.Shutter_enabled,&new_cfg.Shutter_mode,&new_cfg.Shutter_value);
           // gain feature
           this->set_gain(&new_cfg.Gain_enabled,&new_cfg.Gain_mode,&new_cfg.Gain_value);
-          // change the calibration file
-          this->calibration_file=new_cfg.cal_file;
           // update the frame identifier
           this->frame_id=new_cfg.frame_id;
           this->unlock();
@@ -509,12 +515,10 @@ void FirewireCameraDriver::set_gain(bool *enable, int *mode, int *value)
   }
 }
 
-void FirewireCameraDriver::get_image(char **image_data)
+void FirewireCameraDriver::get_image(char *image_data)
 {
   if(this->camera!=NULL)
-    this->camera->get_image(image_data);
-  else
-    *image_data=NULL;
+    this->camera->get_image_shared(image_data);
 }
 
 void FirewireCameraDriver::set_ISO_speed(int iso_speed)
@@ -577,14 +581,15 @@ std::string FirewireCameraDriver::get_new_frame_event(void)
   return event;
 }
 
-std::string FirewireCameraDriver::get_calibration_file(void)
+std::string FirewireCameraDriver::get_frame_id(void)
 {
-  return this->calibration_file;
+  return this->frame_id;
 }
 
-std::string FirewireCameraDriver::get_frame_id(void)
+void FirewireCameraDriver::set_calibration_params(unsigned int width, unsigned int height)
 {
-  return this->frame_id;
+  this->cal_width=width;
+  this->cal_height=height;
 }
 
 FirewireCameraDriver::~FirewireCameraDriver()
diff --git a/src/firewire_camera_driver_node.cpp b/src/firewire_camera_driver_node.cpp
index e11831d..541b979 100644
--- a/src/firewire_camera_driver_node.cpp
+++ b/src/firewire_camera_driver_node.cpp
@@ -1,33 +1,48 @@
 #include "firewire_camera_driver_node.h"
 
-FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>(nh), camera_manager(ros::NodeHandle(this->public_node_handle_))
+FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>(nh), 
+                                                                          camera_manager(ros::NodeHandle(this->public_node_handle_))
 {
   std::string cal_file;
+  bool cal_ok=false;
 
   //init class attributes if necessary
   this->loop_rate_ = 1000;//in [Hz]
   this->desired_framerate=30.0;
   this->diagnosed_camera_image=NULL;
-  this->it=NULL;
 
-  this->it=new image_transport::ImageTransport(this->public_node_handle_);
+  this->camera.it.reset(new image_transport::ImageTransport(this->public_node_handle_));
+  this->camera.Image_msg_=sensor_msgs::ImagePtr(new sensor_msgs::Image);
   this->diagnosed_camera_image = new diagnostic_updater::HeaderlessTopicDiagnostic("camera_image",this->diagnostic_,
                                      diagnostic_updater::FrequencyStatusParam(&this->desired_framerate,&this->desired_framerate,0.01,5));
 
   this->event_server=CEventServer::instance();
 
   // [init publishers]
-  this->camera_image_publisher_ = this->it->advertiseCamera("camera_image", 1);
+  this->camera.camera_image_publisher_ = this->camera.it->advertiseCamera("camera_image", 1);
 
   // try to load the calibration file
   public_node_handle_.param<std::string>("cal_file", cal_file, "");
+  this->camera_manager.setCameraName("firewire_camera");
   if(this->camera_manager.validateURL(cal_file))
   {
-    if(!this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file()))
-      ROS_INFO("Invalid calibration file");
-  } 
+    if(this->camera_manager.loadCameraInfo(cal_file))
+      ROS_INFO("Found calibration file for the camera: %s",cal_file.c_str());
+    else
+      ROS_INFO("Invalid calibration file for the camera");
+    this->camera.CameraInfo_msg_.reset(new sensor_msgs::CameraInfo(this->camera_manager.getCameraInfo()));
+    cal_ok=true;
+  }
   else
-    ROS_INFO("Invalid calibration file");
+  {
+    ROS_INFO("Calibration file does not exist");
+    cal_ok=false;
+  }
+
+  if(!cal_ok)
+    this->driver_.set_calibration_params(-1,-1);
+  else
+    this->driver_.set_calibration_params(this->camera.CameraInfo_msg_->width,this->camera.CameraInfo_msg_->height);
 
   public_node_handle_.param<std::string>("tf_prefix", tf_prefix_, "");
   
@@ -44,7 +59,6 @@ FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_ba
 
 void FirewireCameraDriverNode::mainNodeThread(void)
 {
-  sensor_msgs::CameraInfo camera_info=this->camera_manager.getCameraInfo();
   std::list<std::string> events;
   char *image_data=NULL;
   TCameraConfig config;
@@ -59,48 +73,46 @@ void FirewireCameraDriverNode::mainNodeThread(void)
       {
         events.push_back(this->new_frame_event);
         this->event_server->wait_all(events,1000);
-        this->driver_.get_image(&image_data);
-        if(image_data!=NULL && this->camera_image_publisher_.getNumSubscribers()>0)
+        if(this->camera.camera_image_publisher_.getNumSubscribers()>0)
         {
           this->driver_.get_current_config(&config);
           // update the desired framerate for the diagnostics
           this->desired_framerate=config.framerate;
           //fill msg structures
-          this->Image_msg_.width=config.width;
-          this->Image_msg_.height=config.height;
-          this->Image_msg_.step=config.width*config.depth/8;
-          this->Image_msg_.data=std::vector<unsigned char>(image_data,image_data+config.width*config.height*config.depth/8);
+          this->camera.Image_msg_->width=config.width;
+          this->camera.Image_msg_->height=config.height;
+          this->camera.Image_msg_->step=config.width*config.depth/8;
+          this->camera.Image_msg_->data.resize(config.width*config.height*config.depth/8);
+          this->driver_.get_image((char *)this->camera.Image_msg_->data.data());
           if(config.coding==MONO || config.coding==RAW)
           {
             if(config.depth==DEPTH8)
-              this->Image_msg_.encoding="mono8";
+              this->camera.Image_msg_->encoding="mono8";
             else if(config.depth==DEPTH16)
-              this->Image_msg_.encoding="16UC1";
+              this->camera.Image_msg_->encoding="16UC1";
             else
-              this->Image_msg_.encoding="16UC1";
+              this->camera.Image_msg_->encoding="16UC1";
           }
           else if(config.coding==YUV)
           {
-            this->Image_msg_.encoding="yuv422";
+            this->camera.Image_msg_->encoding="yuv422";
           }
           else
           {
             if(config.depth==DEPTH24)
-              this->Image_msg_.encoding="rgb8";
+              this->camera.Image_msg_->encoding="rgb8";
             else if(config.depth==DEPTH48)
-              this->Image_msg_.encoding="16UC3";
+              this->camera.Image_msg_->encoding="16UC3";
             else
-              this->Image_msg_.encoding="16UC3";
+              this->camera.Image_msg_->encoding="16UC3";
           }
         }
-        delete[] image_data;
         //publish messages
-        this->Image_msg_.header.stamp = ros::Time::now();
-        this->Image_msg_.header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
-        camera_info.header.stamp = this->Image_msg_.header.stamp;
-        camera_info.header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
-        //this->Image_msg_.header.frame_id = "<publisher_topic_name>";
-        this->camera_image_publisher_.publish(this->Image_msg_,camera_info);
+        this->camera.Image_msg_->header.stamp = ros::Time::now();
+        this->camera.Image_msg_->header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
+        this->camera.CameraInfo_msg_->header.stamp = this->camera.Image_msg_->header.stamp;
+        this->camera.CameraInfo_msg_->header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
+        this->camera.camera_image_publisher_.publish(this->camera.Image_msg_,this->camera.CameraInfo_msg_);
         this->diagnosed_camera_image->tick();
       }
     }
@@ -176,23 +188,11 @@ void FirewireCameraDriverNode::addNodeRunningTests(void)
 
 void FirewireCameraDriverNode::reconfigureNodeHook(int level)
 {
-  // try to load the calibration file
-  if(this->camera_manager.validateURL(this->driver_.get_calibration_file()))
-  {
-    this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file());
-  } 
-  else
-    ROS_INFO("Invalid calibration file");
 }
 
 FirewireCameraDriverNode::~FirewireCameraDriverNode()
 {
   // [free dynamic memory]
-  if(this->it!=NULL)
-  {
-    delete this->it;
-    this->it=NULL;
-  }
   if(this->diagnosed_camera_image!=NULL)
   {
     delete this->diagnosed_camera_image;
@@ -205,3 +205,48 @@ int main(int argc,char *argv[])
 {
   return driver_base::main<FirewireCameraDriverNode>(argc,argv,"firewire_camera_driver_node");
 }
+
+#include <pluginlib/class_list_macros.h>
+
+FirewireCameraNodelet::FirewireCameraNodelet()
+{
+  this->node=NULL;
+}
+
+FirewireCameraNodelet::~FirewireCameraNodelet(void)
+{
+  // kill the thread
+  this->thread_server->kill_thread(this->spin_thread_id);
+  this->thread_server->detach_thread(this->spin_thread_id);
+  this->thread_server->delete_thread(this->spin_thread_id);
+  this->spin_thread_id="";
+  // [free dynamic memory]
+  if(this->node!=NULL)
+    delete this->node;
+}
+
+void FirewireCameraNodelet::onInit()
+{
+  // initialize the driver node
+  this->node=new FirewireCameraDriverNode(getPrivateNodeHandle());
+  // initialize the thread
+  this->thread_server=CThreadServer::instance();
+  this->spin_thread_id=getName() + "_bumblebee2_nodelet_spin";
+  this->thread_server->create_thread(this->spin_thread_id);
+  this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);
+  // start the spin thread
+  this->thread_server->start_thread(this->spin_thread_id);
+}
+
+void *FirewireCameraNodelet::spin_thread(void *param)
+{
+  FirewireCameraNodelet *nodelet=(FirewireCameraNodelet *)param;
+
+  nodelet->node->spin();
+
+  pthread_exit(NULL);
+}
+
+// parameters are: package, class name, class type, base class type
+PLUGINLIB_DECLARE_CLASS(iri_firewire_camera, FirewireCameraNodelet, FirewireCameraNodelet, nodelet::Nodelet);
+
diff --git a/src/firewire_camera_driver_nodelet.cpp b/src/firewire_camera_driver_nodelet.cpp
deleted file mode 100755
index 1979648..0000000
--- a/src/firewire_camera_driver_nodelet.cpp
+++ /dev/null
@@ -1,246 +0,0 @@
-#include "firewire_camera_driver_nodelet.h"
-#include <pluginlib/class_list_macros.h>
-
-FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>(nh), 
-                                                                          camera_manager(ros::NodeHandle(this->public_node_handle_)),
-                                                                          it(new image_transport::ImageTransport(this->public_node_handle_)),
-                                                                          Image_msg_(new sensor_msgs::Image)
-{
-  std::string cal_file;
-
-  //init class attributes if necessary
-  this->loop_rate_ = 1000;//in [Hz]
-  this->desired_framerate=30.0;
-  this->diagnosed_camera_image=NULL;
-
-  this->diagnosed_camera_image = new diagnostic_updater::HeaderlessTopicDiagnostic("camera_image",this->diagnostic_,
-                                     diagnostic_updater::FrequencyStatusParam(&this->desired_framerate,&this->desired_framerate,0.01,5));
-
-  this->event_server=CEventServer::instance();
-
-  // [init publishers]
-  this->camera_image_publisher_ = this->it->advertiseCamera("camera_image", 1);
-
-  // try to load the calibration file
-  public_node_handle_.param<std::string>("left_cal_file", cal_file, "");
-  if(this->camera_manager.validateURL(cal_file))
-  {
-    if(!this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file()))
-      ROS_INFO("Invalid calibration file");
-  } 
-  else
-    ROS_INFO("Invalid calibration file");
-
-  public_node_handle_.param<std::string>("tf_prefix", tf_prefix_, "");
-  
-  // [init subscribers]
-  
-  // [init services]
-  
-  // [init clients]
-  
-  // [init action servers]
-  
-  // [init action clients]
-}
-
-void FirewireCameraDriverNode::mainNodeThread(void)
-{
-  sensor_msgs::CameraInfoPtr camera_info(new sensor_msgs::CameraInfo(this->camera_manager.getCameraInfo()));
-  std::list<std::string> events;
-  char *image_data=NULL;
-  TCameraConfig config;
-
-  try{
-    //lock access to driver if necessary
-    this->driver_.lock();
-    if(this->driver_.isRunning())
-    {
-      this->new_frame_event=this->driver_.get_new_frame_event();
-      if(this->new_frame_event!="")
-      {
-        events.push_back(this->new_frame_event);
-        this->event_server->wait_all(events,1000);
-        this->driver_.get_image(&image_data);
-        if(image_data!=NULL && this->camera_image_publisher_.getNumSubscribers()>0)
-        {
-          this->driver_.get_current_config(&config);
-          // update the desired framerate for the diagnostics
-          this->desired_framerate=config.framerate;
-          //fill msg structures
-          this->Image_msg_->width=config.width;
-          this->Image_msg_->height=config.height;
-          this->Image_msg_->step=config.width*config.depth/8;
-          this->Image_msg_->data=std::vector<unsigned char>(image_data,image_data+config.width*config.height*config.depth/8);
-          if(config.coding==MONO)
-          {
-            if(config.depth==DEPTH8)
-              this->Image_msg_->encoding="8UC1";
-            else if(config.depth==DEPTH16)
-              this->Image_msg_->encoding="16UC1";
-            else
-              this->Image_msg_->encoding="16UC1";
-          }
-          else if(config.coding==RAW)
-          {
-            this->Image_msg_->encoding="bayer_rggb8";
-          }
-          else if(config.coding==YUV)
-          {
-            this->Image_msg_->encoding="yuv422";
-          }
-          else
-          {
-            if(config.depth==DEPTH24)
-              this->Image_msg_->encoding="rgb8";
-            else if(config.depth==DEPTH48)
-              this->Image_msg_->encoding="16UC3";
-            else
-              this->Image_msg_->encoding="16UC3";
-          }
-        }
-        delete[] image_data;
-        //publish messages
-        this->Image_msg_->header.stamp = ros::Time::now();
-        this->Image_msg_->header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
-        camera_info->header.stamp = this->Image_msg_->header.stamp;
-        camera_info->header.frame_id = tf_prefix_ + "/" + this->driver_.get_frame_id();
-        //this->Image_msg_.header.frame_id = "<publisher_topic_name>";
-        this->camera_image_publisher_.publish(this->Image_msg_,camera_info);
-        this->diagnosed_camera_image->tick();
-      }
-    }
-    this->driver_.unlock();
-  }catch(CException &e){
-    this->driver_.unlock();
-    ROS_INFO("Impossible to capture frame");
-  }
-  //fill srv structure and make request to the server
-}
-  // [fill msg Header if necessary]
-
-void FirewireCameraDriverNode::check_configuration(diagnostic_updater::DiagnosticStatusWrapper &stat)
-{
-  TCameraConfig desired_config;
-  TCameraConfig current_config;
-
-  this->driver_.get_current_config(&current_config);
-  this->driver_.get_desired_config(&desired_config);
-
-  if(current_config.left_offset!=desired_config.left_offset)
-  {
-    stat.summary(1,"The current configuration differs from the desired configuration");
-    stat.addf("Image left offset:","current value is %d,desired value is %d", current_config.left_offset,desired_config.left_offset);
-  }
-  else if(current_config.top_offset!=desired_config.top_offset)
-  {
-    stat.summary(1,"The current configuration differs from the desired configuration");
-    stat.addf("Image top offset:","current value is %d,desired value is %d", current_config.top_offset,desired_config.top_offset);
-  }
-  else if(current_config.width!=desired_config.width)
-  {
-    stat.summary(1,"The current configuration differs from the desired configuration");
-    stat.addf("Image width:","current value is %d,desired value is %d", current_config.width,desired_config.width);
-  }
-  else if(current_config.height!=desired_config.height)
-  {
-    stat.summary(1,"The current configuration differs from the desired configuration");
-    stat.addf("Image height:","current value is %d,desired value is %d", current_config.height,desired_config.height);
-  }
-  else
-    stat.summary(0,"The current configuration is okay");
-}
-
-/*  [subscriber callbacks] */
-
-/*  [service callbacks] */
-
-/*  [action callbacks] */
-
-/*  [action requests] */
-
-void FirewireCameraDriverNode::postNodeOpenHook(void)
-{
-}
-
-void FirewireCameraDriverNode::addNodeDiagnostics(void)
-{
-  diagnostic_.add("Check Configuration", this, &FirewireCameraDriverNode::check_configuration);
-}
-
-void FirewireCameraDriverNode::addNodeOpenedTests(void)
-{
-}
-
-void FirewireCameraDriverNode::addNodeStoppedTests(void)
-{
-}
-
-void FirewireCameraDriverNode::addNodeRunningTests(void)
-{
-}
-
-void FirewireCameraDriverNode::reconfigureNodeHook(int level)
-{
-  // try to load the calibration file
-  if(this->camera_manager.validateURL(this->driver_.get_calibration_file()))
-  {
-    this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file());
-  } 
-  else
-    ROS_INFO("Invalid calibration file");
-}
-
-FirewireCameraDriverNode::~FirewireCameraDriverNode()
-{
-  // [free dynamic memory]
-  if(this->diagnosed_camera_image!=NULL)
-  {
-    delete this->diagnosed_camera_image;
-    this->diagnosed_camera_image=NULL;
-  }
-}
-
-#include "firewire_camera_driver_nodelet.h"
-#include <pluginlib/class_list_macros.h>
-
-FirewireCameraNodelet::FirewireCameraNodelet()
-{
-  this->dev=NULL;
-}
-
-void FirewireCameraNodelet::onInit()
-{
-  this->dev=new FirewireCameraDriverNode(getPrivateNodeHandle());
-  // initialize the thread
-  this->thread_server=CThreadServer::instance();
-  this->spin_thread_id=getName() + "_camera_driver_nodelet_spin";
-  this->thread_server->create_thread(this->spin_thread_id);
-  this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);
-  // start the spin thread
-  this->thread_server->start_thread(this->spin_thread_id);
-}
-
-void *FirewireCameraNodelet::spin_thread(void *param)
-{
-  FirewireCameraNodelet *nodelet=(FirewireCameraNodelet *)param;
-
-  nodelet->dev->spin();
-
-  pthread_exit(NULL);
-}
-
-FirewireCameraNodelet::~FirewireCameraNodelet()
-{
-  // kill the thread
-  this->thread_server->kill_thread(this->spin_thread_id);
-  this->thread_server->detach_thread(this->spin_thread_id);
-  this->thread_server->delete_thread(this->spin_thread_id);
-  this->spin_thread_id="";
-  if(this->dev!=NULL)
-    delete this->dev;
-}
-
-// parameters are: package, class name, class type, base class type
-PLUGINLIB_DECLARE_CLASS(iri_firewire_camera, FirewireCameraNodelet, FirewireCameraNodelet, nodelet::Nodelet);
-
-- 
GitLab