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(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) diff --git a/src/nodes/features.h b/src/nodes/features.h index 62743bc..add4261 100644 --- a/src/nodes/features.h +++ b/src/nodes/features.h @@ -41,10 +41,9 @@ #include #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 diff --git a/src/nodes/trigger.cpp b/src/nodes/trigger.cpp index f98f9f2..8a75e67 100644 --- a/src/nodes/trigger.cpp +++ b/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); } diff --git a/src/nodes/trigger.h b/src/nodes/trigger.h index 2a7ceb3..972293b 100644 --- a/src/nodes/trigger.h +++ b/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);