add configurable output rate, different from camera poll rate

src/nodes/dev_camera1394.cpp

@@ -448,6 +448,32 @@ std::string bayer_string(dc1394color_filter_t pattern, unsigned int bits)
   return sensor_msgs::image_encodings::MONO8;
 }
 
+/** Poll sensor without reading data */
+bool Camera1394::pollNoRead()
+{
+  dc1394video_frame_t * frame = NULL;
+
+  if (features_->isTriggerPowered())
+  {
+    ROS_DEBUG("[%016lx] polling camera", camera_->guid);
+    dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_POLL, &frame);
+    if (!frame) return false;
+  }
+  else
+  {
+    ROS_DEBUG("[%016lx] waiting camera", camera_->guid);
+    dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
+    if (!frame)
+    {
+      CAM_EXCEPT(camera1394::Exception, "Unable to capture frame");
+      return false;
+    }
+  }
+
+  dc1394_capture_enqueue(camera_, frame);
+  return true;
+}
+
 /** Return an image frame */
 bool Camera1394::readData(sensor_msgs::Image& image)
 {

src/nodes/dev_camera1394.h

@@ -76,6 +76,7 @@ namespace camera1394
     int open(camera1394::Camera1394Config &newconfig);
     int close();
     bool readData (sensor_msgs::Image &image);
+    bool pollNoRead();
 
     /** check whether CameraInfo matches current video mode
      *

src/nodes/driver1394.cpp

@@ -92,11 +92,14 @@ namespace camera1394_driver
     topic_diagnostics_min_freq_(0.),
     topic_diagnostics_max_freq_(1000.),
     topic_diagnostics_("image_raw", diagnostics_,
-		       diagnostic_updater::FrequencyStatusParam
-		       (&topic_diagnostics_min_freq_,
-			&topic_diagnostics_max_freq_, 0.1, 10),
-		       diagnostic_updater::TimeStampStatusParam())
-  {}
+               diagnostic_updater::FrequencyStatusParam
+               (&topic_diagnostics_min_freq_,
+            &topic_diagnostics_max_freq_, 0.1, 10),
+               diagnostic_updater::TimeStampStatusParam())
+  {
+    priv_nh_.param<float>( "output_rate_hz", output_rate_hz_, 0 ); // 0 to disable
+    ROS_INFO("%s: output rate=%.2fHz", ROS_PACKAGE_NAME, output_rate_hz_ );
+  }
 
   Camera1394Driver::~Camera1394Driver()
   {}
@@ -155,6 +158,8 @@ namespace camera1394_driver
             calibration_matches_ = true;
             newconfig.guid = camera_name_; // update configuration parameter
             retries_ = 0;
+            camera_time_ref_ = ros::Time::now();
+            output_frame_count_ = 1;
             success = true;
           }
       }
@@ -192,6 +197,7 @@ namespace camera1394_driver
     // seconds before getting to run.  So, we avoid acquiring the lock
     // if there is a reconfig() pending.
     bool do_sleep = true;
+
     if (!reconfiguring_)
       {
         boost::mutex::scoped_lock lock(mutex_);
@@ -202,17 +208,31 @@ namespace camera1394_driver
         do_sleep = (state_ == Driver::CLOSED);
         if (!do_sleep)                  // openCamera() succeeded?
           {
-            // driver is open, read the next image still holding lock
-            sensor_msgs::ImagePtr image(new sensor_msgs::Image);
-            if (read(image))
-              {
-                publish(image);
-                consecutive_read_errors_ = 0;
-              }
-            else if ( ++consecutive_read_errors_ > config_.max_consecutive_errors && config_.max_consecutive_errors > 0 )
+			float time_elapsed = ( ros::Time::now() - camera_time_ref_ ).toSec();
+
+            // If we need a new image to achieve the requested output 
+            // rate (or if disabled =0Hz) then read and publish the image. 
+            // Otherwise, just poll the camera without processing image data. 
+
+            if ( output_rate_hz_ > 0 && time_elapsed * output_rate_hz_ >= output_frame_count_ )
+            {
+                sensor_msgs::ImagePtr image(new sensor_msgs::Image);
+                if ( read(image) ) 
+                { 
+                  publish(image); 
+                  consecutive_read_errors_ = 0; 
+                  ++output_frame_count_;
+                }
+                else if ( ++consecutive_read_errors_ > config_.max_consecutive_errors && config_.max_consecutive_errors > 0 )
+                {
+                  ROS_WARN("reached %lu consecutive read errrors, disconnecting", consecutive_read_errors_ );
+                  closeCamera();
+                }
+            }
+            else // poll camera without processing or publishing frame
             {
-              ROS_WARN("reached %lu consecutive read errrors, disconnecting", consecutive_read_errors_ );
-              closeCamera();
+                try { dev_->pollNoRead(); }
+                catch( const std::exception& e ) { ROS_WARN_THROTTLE( 1, "Failed to poll camera, error=%s", e.what() ); } 
             }
           }
       } // release mutex lock

src/nodes/driver1394.h

@@ -93,12 +93,15 @@ class Camera1394Driver
 
   /** driver state variables */
   volatile driver_base::Driver::state_t state_; // current driver state
-  volatile bool reconfiguring_;         // true when reconfig() running
+  volatile bool reconfiguring_;        // true when reconfig() running
   ros::NodeHandle priv_nh_;             // private node handle
   ros::NodeHandle camera_nh_;           // camera name space handle
   std::string camera_name_;             // camera name
   ros::Rate cycle_;                     // polling rate when closed
-  uint32_t retries_;                    // count of openCamera() retries
+  uint32_t retries_;                   // count of openCamera() retries
+  uint32_t output_frame_count_;			// output frame count
+  float output_rate_hz_;               // output (published) image rate in Hz (0 to disable)
+  ros::Time camera_time_ref_;           // time camera connection was created
   uint32_t consecutive_read_errors_;   // number of consecutive read errors
 
   /** libdc1394 camera device interface */