From d279c2d3a18a5cb59cadc17c74ca03a44588f496 Mon Sep 17 00:00:00 2001
From: Boris Gromov <0xff.root@gmail.com>
Date: Tue, 3 Sep 2013 22:38:49 +0900
Subject: [PATCH] Poll camera whenever external trigger is ON

---
 src/nodes/dev_camera1394.cpp | 24 ++++++++++++++++++------
 src/nodes/dev_camera1394.h   |  2 +-
 src/nodes/driver1394.cpp     |  2 +-
 3 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/src/nodes/dev_camera1394.cpp b/src/nodes/dev_camera1394.cpp
index 9c118a6..0a536fd 100644
--- a/src/nodes/dev_camera1394.cpp
+++ b/src/nodes/dev_camera1394.cpp
@@ -449,17 +449,27 @@ std::string bayer_string(dc1394color_filter_t pattern, unsigned int bits)
 }
 
 /** Return an image frame */
-void Camera1394::readData(sensor_msgs::Image& image)
+bool Camera1394::readData(sensor_msgs::Image& image)
 {
   ROS_ASSERT_MSG(camera_, "Attempt to read from camera that is not open.");
 
   dc1394video_frame_t * frame = NULL;
-  dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
-  if (!frame)
+  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;
+      return false;
     }
+  }
   
   uint8_t* capture_buffer;
 
@@ -486,7 +496,7 @@ void Camera1394::readData(sensor_msgs::Image& image)
           free(frame2.image);
           dc1394_capture_enqueue(camera_, frame);
           CAM_EXCEPT(camera1394::Exception, "Could not convert/debayer frames");
-          return;
+          return false;
         }
 
       capture_buffer = reinterpret_cast<uint8_t *>(frame2.image);
@@ -604,11 +614,13 @@ void Camera1394::readData(sensor_msgs::Image& image)
       else
         {
           CAM_EXCEPT(camera1394::Exception, "Unknown image mode");
-          return;
+          return false;
         }
     }
   dc1394_capture_enqueue(camera_, frame);
 
   if (DoBayerConversion_) 
     free(capture_buffer);
+
+  return true;
 }
diff --git a/src/nodes/dev_camera1394.h b/src/nodes/dev_camera1394.h
index c4c4010..a43024a 100644
--- a/src/nodes/dev_camera1394.h
+++ b/src/nodes/dev_camera1394.h
@@ -74,7 +74,7 @@ namespace camera1394
 
     int open(camera1394::Camera1394Config &newconfig);
     int close();
-    void readData (sensor_msgs::Image &image);
+    bool readData (sensor_msgs::Image &image);
 
     /** check whether CameraInfo matches current video mode
      *
diff --git a/src/nodes/driver1394.cpp b/src/nodes/driver1394.cpp
index 65bfeb6..860c584 100644
--- a/src/nodes/driver1394.cpp
+++ b/src/nodes/driver1394.cpp
@@ -278,7 +278,7 @@ namespace camera1394_driver
       {
         // Read data from the Camera
         ROS_DEBUG_STREAM("[" << camera_name_ << "] reading data");
-        dev_->readData(*image);
+        success = dev_->readData(*image);
         ROS_DEBUG_STREAM("[" << camera_name_ << "] read returned");
       }
     catch (camera1394::Exception& e)