Merge pull request #25 from bgromov/camera_poll

Poll camera whenever external trigger is ON (fix #22)

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;
 }

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
      *

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)

src/nodes/features.h

@@ -41,10 +41,9 @@
 #include <dc1394/dc1394.h>
 
 #include "camera1394/Camera1394Config.h"
+#include "trigger.h"
 typedef camera1394::Camera1394Config Config;
 
-class Trigger;                        // actually defined in trigger.h
-
 /** @file
 
     @brief Camera1394 features interface
@@ -68,6 +67,11 @@ class Features
   bool initialize(Config *newconfig);
   void reconfigure(Config *newconfig);
 
+  inline bool isTriggerPowered()
+  {
+	  return trigger_->isPowered();
+  }
+
 private:
   typedef int state_t;      ///< camera1394::Camera1394_* state values
 

src/nodes/trigger.cpp

@@ -243,6 +243,7 @@ dc1394switch_t Trigger::getExternalTriggerPowerState(dc1394camera_t *camera)
     ROS_FATAL("getExternalTriggerPowerState() failed: %d", err);
     return (dc1394switch_t)-1; // failure
   }
+  externalTriggerPowerState_ = state;
   return state;
 }
 
@@ -267,6 +268,7 @@ bool Trigger::setExternalTriggerPowerState(dc1394camera_t *camera, dc1394switch_
     ROS_FATAL("setExternalTriggerPowerState() failed: %d", err);
     return false; // failure
   }
+  externalTriggerPowerState_ = state;
   ROS_DEBUG("setExternalTriggerPowerState(): %s", (state == DC1394_ON ? "ON" : "OFF"));
   return true; // success
 }
@@ -514,6 +516,9 @@ bool Trigger::initialize(Config *newconfig)
     return false;
   }
 
+  // Update externalTriggerPowerState_ variable with current value
+  Trigger::getExternalTriggerPowerState(camera_);
+
   // configure trigger features
   return reconfigure(newconfig);
 }

src/nodes/trigger.h

@@ -69,6 +69,8 @@ class Trigger
   dc1394trigger_sources_t triggerSources_;
   dc1394trigger_polarity_t triggerPolarity_;
 
+  dc1394switch_t externalTriggerPowerState_;
+
   bool findTriggerMode(std::string str);
   bool findTriggerSource(std::string str);
   bool findTriggerPolarity(std::string str);
@@ -80,7 +82,7 @@ class Trigger
    *  @param camera address of DC1394 camera structure.
    */
   Trigger(dc1394camera_t *camera):
-    camera_(camera), triggerSources_((dc1394trigger_sources_t){0})
+    camera_(camera), triggerSources_((dc1394trigger_sources_t){0}), externalTriggerPowerState_(DC1394_OFF)
   {};
 
   /** Return driver parameter name of DC1394 trigger_mode.
@@ -122,6 +124,17 @@ class Trigger
       return "";
   }
 
+  /** Checks whether external trigger power is ON or OFF.
+   *  This method uses cached value, which is updated every
+   *  time the settings are changed by user
+   *
+   *  @return true if external trigger power is ON; false if not
+   */
+  inline bool isPowered()
+  {
+    return (externalTriggerPowerState_ == DC1394_ON ? true : false);
+  }
+
   bool enumSources(dc1394camera_t *camera, dc1394trigger_sources_t &sources);
   dc1394trigger_polarity_t getPolarity(dc1394camera_t *camera);
   bool setPolarity(dc1394camera_t *camera, dc1394trigger_polarity_t &polarity);