Poll camera whenever external trigger is ON

ros-drivers · Sep 3, 2013 · d279c2d · d279c2d
1 parent 3d63c80
commit d279c2d
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 8 deletions.
diff --git 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
@@ -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
@@ -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)