diff --git a/src/nodes/dev_camera1394.cpp b/src/nodes/dev_camera1394.cpp index 7f4bdf8..842bdbc 100644 --- a/src/nodes/dev_camera1394.cpp +++ b/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) { diff --git a/src/nodes/dev_camera1394.h b/src/nodes/dev_camera1394.h index 339e452..b8350ca 100644 --- a/src/nodes/dev_camera1394.h +++ b/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 * diff --git a/src/nodes/driver1394.cpp b/src/nodes/driver1394.cpp index 9b32057..9575112 100644 --- a/src/nodes/driver1394.cpp +++ b/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( "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 diff --git a/src/nodes/driver1394.h b/src/nodes/driver1394.h index 28384c5..c2fecbf 100644 --- a/src/nodes/driver1394.h +++ b/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 */