From 0433b56a1de91d2c5ce0ba45388b347c4afb26b5 Mon Sep 17 00:00:00 2001 From: francois Date: Wed, 18 May 2016 10:54:59 +0200 Subject: [PATCH 1/2] add configurable output rate, different from camera poll rate --- src/nodes/dev_camera1394.cpp | 26 ++++++++++++++++++++++++++ src/nodes/dev_camera1394.h | 1 + src/nodes/driver1394.cpp | 31 ++++++++++++++++++++++++------- src/nodes/driver1394.h | 2 ++ 4 files changed, 53 insertions(+), 7 deletions(-) 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 986287b..e22a9f1 100644 --- a/src/nodes/driver1394.cpp +++ b/src/nodes/driver1394.cpp @@ -75,6 +75,7 @@ namespace camera1394_driver camera_name_("camera"), cycle_(1.0), // slow poll when closed retries_(0), + last_camera_poll_(0), dev_(new camera1394::Camera1394()), srv_(priv_nh), cinfo_(new camera_info_manager::CameraInfoManager(camera_nh_)), @@ -95,7 +96,10 @@ namespace camera1394_driver (&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() {} @@ -190,6 +194,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_); @@ -200,12 +205,24 @@ 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); - } + ros::Time time_now = ros::Time::now(); + float rate_hz = 1.0 / ( time_now - last_camera_poll_ ).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 || rate_hz <= output_rate_hz_ ) + { + sensor_msgs::ImagePtr image(new sensor_msgs::Image); + if ( read(image) ) { publish(image); last_camera_poll_ = time_now; } + else ROS_WARN_THROTTLE( 1, "Failed to read image" ); + } + else // poll camera without processing or publishing frame + { + 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 cfa80be..aaf8cd7 100644 --- a/src/nodes/driver1394.h +++ b/src/nodes/driver1394.h @@ -99,6 +99,8 @@ class Camera1394Driver std::string camera_name_; // camera name ros::Rate cycle_; // polling rate when closed uint32_t retries_; // count of openCamera() retries + float output_rate_hz_; // output (published) image rate in Hz (0 to disable) + ros::Time last_camera_poll_; // timestamp of last time camera was polled /** libdc1394 camera device interface */ boost::shared_ptr dev_; From 515735f9ce37f9b873631f6117fa02ba4e61995d Mon Sep 17 00:00:00 2001 From: francois Date: Mon, 23 May 2016 15:01:40 +0200 Subject: [PATCH 2/2] fixed indents --- cfg/Camera1394.cfg | 3 ++ src/nodes/driver1394.cpp | 61 ++++++++++++++++++++++++---------------- src/nodes/driver1394.h | 12 ++++---- 3 files changed, 46 insertions(+), 30 deletions(-) diff --git a/cfg/Camera1394.cfg b/cfg/Camera1394.cfg index 0d3ab5f..8afe582 100755 --- a/cfg/Camera1394.cfg +++ b/cfg/Camera1394.cfg @@ -89,6 +89,9 @@ gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_CLOSE, gen.add("frame_rate", double_t, SensorLevels.RECONFIGURE_CLOSE, "Camera speed (frames per second).", 15.0, 1.875, 240.0) +gen.add("max_consecutive_errors", int_t, SensorLevels.RECONFIGURE_CLOSE, + "Max number of consecutive read errors before attempting reconnection (0 to disable).", 0, 0, 1000 ) + gen.add("iso_speed", int_t, SensorLevels.RECONFIGURE_CLOSE, "Total IEEE 1394 bus bandwidth (Megabits/second).", 400, 100, 3200) diff --git a/src/nodes/driver1394.cpp b/src/nodes/driver1394.cpp index e22a9f1..9575112 100644 --- a/src/nodes/driver1394.cpp +++ b/src/nodes/driver1394.cpp @@ -75,7 +75,7 @@ namespace camera1394_driver camera_name_("camera"), cycle_(1.0), // slow poll when closed retries_(0), - last_camera_poll_(0), + consecutive_read_errors_(0), dev_(new camera1394::Camera1394()), srv_(priv_nh), cinfo_(new camera_info_manager::CameraInfoManager(camera_nh_)), @@ -92,13 +92,13 @@ 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_ ); + 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() @@ -158,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; } } @@ -179,6 +181,7 @@ namespace camera1394_driver topic_diagnostics_min_freq_ = newconfig.frame_rate - delta; topic_diagnostics_max_freq_ = newconfig.frame_rate + delta; + consecutive_read_errors_ = 0; return success; } @@ -205,24 +208,32 @@ namespace camera1394_driver do_sleep = (state_ == Driver::CLOSED); if (!do_sleep) // openCamera() succeeded? { - ros::Time time_now = ros::Time::now(); - float rate_hz = 1.0 / ( time_now - last_camera_poll_ ).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 || rate_hz <= output_rate_hz_ ) - { - sensor_msgs::ImagePtr image(new sensor_msgs::Image); - if ( read(image) ) { publish(image); last_camera_poll_ = time_now; } - else ROS_WARN_THROTTLE( 1, "Failed to read image" ); - } - else // poll camera without processing or publishing frame - { - try { dev_->pollNoRead(); } - catch( const std::exception& e ) { ROS_WARN_THROTTLE( 1, "Failed to poll camera, error=%s", e.what() ); } - } + 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 + { + 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 aaf8cd7..c2fecbf 100644 --- a/src/nodes/driver1394.h +++ b/src/nodes/driver1394.h @@ -93,15 +93,17 @@ 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 - float output_rate_hz_; // output (published) image rate in Hz (0 to disable) - ros::Time last_camera_poll_; // timestamp of last time camera was polled - + 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 */ boost::shared_ptr dev_;