Added a new header with common data structures for all firewire camera types.

The image topic only publishes when there is at least a subscriber.
Solved some minor bugs with the calibration files.

calibration/bumblebee_right.yml

-image_width: 1024
-image_height: 768
-camera_name: camera
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [832.936650996004, 0, 518.836783944374, 0, 834.941863569586, 396.527273711983, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [-0.315637247105216, 0.0599722024900246, -0.000646373979576491, 0.000485253734406295, 0]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [708.893756960653, 0, 522.255043638676, 0, 0, 771.716439056102, 397.789380725747, 0, 0, 0, 1, 0]

calibration/bumblebee_right_safe.yml

-image_width: 1024
-image_height: 768
-camera_name: camera
-camera_matrix:
-  rows: 3
-  cols: 3
-  data: [832.936650996004, 0, 518.836783944374, 0, 834.941863569586, 396.527273711983, 0, 0, 1]
-distortion_model: plumb_bob
-distortion_coefficients:
-  rows: 1
-  cols: 5
-  data: [-0.315637247105216, 0.0599722024900246, -0.000646373979576491, 0.000485253734406295, 0]
-rectification_matrix:
-  rows: 3
-  cols: 3
-  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
-projection_matrix:
-  rows: 3
-  cols: 4
-  data: [708.893756960653, 0, 522.255043638676, 0, 0, 771.716439056102, 397.789380725747, 0, 0, 0, 1, 0]
\ No newline at end of file

include/camera_common.h

+#ifndef _CAMERA_COMMON_H
+#define _CAMERA_COMMON_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>
+
+typedef struct
+{
+  unsigned int width;
+  unsigned int height;
+  unsigned int left_offset;
+  unsigned int top_offset;
+  depths_t depth;
+  codings_t coding;
+  float framerate;
+}TCameraConfig;
+
+typedef struct
+{
+  image_transport::ImageTransport *it;
+  image_transport::CameraPublisher camera_image_publisher_;
+  sensor_msgs::Image Image_msg_;
+  sensor_msgs::CameraInfo CameraInfo_msg_;
+}TROSCamera;
+
+#endif

include/firewire_camera_driver.h

@@ -31,17 +31,7 @@
 //include firewire_camera_driver main library
 #include "firewirecamera.h"
 #include "firewireserver.h"
-
-typedef struct
-{
-  unsigned int width;
-  unsigned int height;
-  unsigned int left_offset;
-  unsigned int top_offset;
-  depths_t depth;
-  codings_t coding;
-  float framerate;
-}TCameraConfig;
+#include "camera_common.h"
 
 /**
  * \brief IRI ROS Specific Driver Class

launch/bumblebee.launch

-<launch>
-
-  <!-- bumblebee -->
-  <node pkg ="iri_firewire_camera"
-        type="iri_firewire_camera"
-        name="iri_firewire_camera"
-        output="screen">
-    <param name="Camera_node"  value="0" />
-    <param name="frame_id"     value="bumblebee_frame" type="string" />
-    <param name="cal_file"     value="file://$(find iri_firewire_camera)/calibration/bumblebee_right.yml" type="string" />
-    <param name="ISO_speed"    value="400" />
-    <param name="Framerate"    value="7.5" />
-    <param name="Color_coding" value="3" />
-    <param name="Image_width"  value="1024" />
-    <param name="Image_height" value="768" />
-  </node>
-
-  <!-- image rectification -->
-  <node pkg ="nodelet"
-        type="nodelet"
-        name="image_proc_dec"
-        args="standalone image_proc/crop_decimate">
-    <param name="decimation_x" value="2" />
-    <param name="decimation_y" value="2" />
-    <remap from="/camera/image_raw"
-             to="/iri_firewire_camera/camera_image" />
-    <remap from="/camera/camera_info"
-             to="/iri_firewire_camera/camera_info" />
-  </node>
-
-  <group ns="camera" >
-    <node pkg = "image_proc"
-          type = "image_proc"
-          name = "image_proc">
-      <remap from="/camera/image_raw"
-               to="/camera_out/image_raw" />
-      <remap from="/camera/camera_info"
-               to="/camera_out/camera_info" />
-    </node>
-  </group>
-
-  <!-- image view -->
-  <node pkg ="image_view"
-        type="image_view"
-        name="image_view"
-        output="screen">
-    <remap from="image"
-             to="/camera/image_rect_color" />
-  </node>
-
-</launch>

launch/camera0rgb.launch

 <launch>
 
-       <node name="iri_firewire_camera" pkg="iri_firewire_camera" type="firewire_camera_node" output="screen">
+       <node name="iri_firewire_camera" pkg="iri_firewire_camera" type="iri_firewire_camera" output="screen">
        <param name="Camera_node" value="0" />
        <param name="Framerate" value="30" />
        <param name="Color_coding" value="3" />

manifest.xml

@@ -17,6 +17,8 @@
   
   <export>
     <nodelet plugin="${prefix}/firewire_nodelet_plugin.xml"/>
+    <cpp cflags="-I${prefix}/include"
+         lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -liri_base_driver"/>
   </export>
 
 </package>

src/firewire_camera_driver_node.cpp

@@ -2,6 +2,8 @@
 
 FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<FirewireCameraDriver>(nh), camera_manager(ros::NodeHandle(this->public_node_handle_))
 {
+  std::string cal_file;
+
   //init class attributes if necessary
   this->loop_rate_ = 1000;//in [Hz]
   this->desired_framerate=30.0;
@@ -18,9 +20,11 @@ FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_ba
   this->camera_image_publisher_ = this->it->advertiseCamera("camera_image", 1);
 
   // try to load the calibration file
-  if(this->camera_manager.validateURL(this->driver_.get_calibration_file()))
+  public_node_handle_.param<std::string>("left_cal_file", cal_file, "");
+  if(this->camera_manager.validateURL(cal_file))
   {
-    this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file());
+    if(!this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file()))
+      ROS_INFO("Invalid calibration file");
   } 
   else
     ROS_INFO("Invalid calibration file");
@@ -50,47 +54,55 @@ void FirewireCameraDriverNode::mainNodeThread(void)
     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->new_frame_event=this->driver_.get_new_frame_event();
+      if(this->new_frame_event!="")
       {
-        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 || config.coding==RAW)
+        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(config.depth==DEPTH8)
-            this->Image_msg_.encoding="8UC1";
-          else if(config.depth==DEPTH16)
-            this->Image_msg_.encoding="16UC1";
+          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 || config.coding==RAW)
+          {
+            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==YUV)
+          {
+            this->Image_msg_.encoding="yuv422";
+          }
           else
-            this->Image_msg_.encoding="16UC1";
-        }
-        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";
+          {
+            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();
       }
-      delete[] image_data;
-    }
     }
     this->driver_.unlock();
   }catch(CException &e){
@@ -98,15 +110,6 @@ void FirewireCameraDriverNode::mainNodeThread(void)
     ROS_INFO("Impossible to capture frame");
   }
   //fill srv structure and make request to the server
-
-  //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();
 }
   // [fill msg Header if necessary]
 

src/firewire_camera_driver_nodelet.cpp

@@ -4,6 +4,8 @@
 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;
@@ -18,9 +20,11 @@ FirewireCameraDriverNode::FirewireCameraDriverNode(ros::NodeHandle &nh) : iri_ba
   this->camera_image_publisher_ = this->it->advertiseCamera("camera_image", 1);
 
   // try to load the calibration file
-  if(this->camera_manager.validateURL(this->driver_.get_calibration_file()))
+  public_node_handle_.param<std::string>("left_cal_file", cal_file, "");
+  if(this->camera_manager.validateURL(cal_file))
   {
-    this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file());
+    if(!this->camera_manager.loadCameraInfo(this->driver_.get_calibration_file()))
+      ROS_INFO("Invalid calibration file");
   } 
   else
     ROS_INFO("Invalid calibration file");
@@ -50,47 +54,55 @@ void FirewireCameraDriverNode::mainNodeThread(void)
     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->new_frame_event=this->driver_.get_new_frame_event();
+      if(this->new_frame_event!="")
       {
-        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 || config.coding==RAW)
+        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(config.depth==DEPTH8)
-            this->Image_msg_->encoding="8UC1";
-          else if(config.depth==DEPTH16)
-            this->Image_msg_->encoding="16UC1";
+          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 || config.coding==RAW)
+          {
+            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==YUV)
+          {
+            this->Image_msg_->encoding="yuv422";
+          }
           else
-            this->Image_msg_->encoding="16UC1";
-        }
-        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";
+          {
+            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();
       }
-      delete[] image_data;
-    }
     }
     this->driver_.unlock();
   }catch(CException &e){
@@ -98,15 +110,6 @@ void FirewireCameraDriverNode::mainNodeThread(void)
     ROS_INFO("Impossible to capture frame");
   }
   //fill srv structure and make request to the server
-
-  //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();
 }
   // [fill msg Header if necessary]