From bb723e2a1725d62fec6e6e7ce42020c9f3ed6a83 Mon Sep 17 00:00:00 2001 From: Brian Lee Date: Fri, 30 Mar 2018 13:49:12 -0700 Subject: [PATCH 1/2] Fetch the cameras's timestamp associated with an image Add an option to fetch the timestamp of when the frame acquision was triggered along with the image. This is useful for applications that require very accurate timestamps when the camera supports PTP IEEE-1588 synchronization. This timestamp is grabbed using the Basler pylon API's chunk feature. --- config/default.yaml | 11 +++ .../internal/impl/pylon_camera_base.hpp | 71 +++++++++++++++++++ .../internal/impl/pylon_camera_gige.hpp | 1 + .../internal/impl/pylon_camera_usb.hpp | 1 + include/pylon_camera/internal/pylon_camera.h | 5 ++ include/pylon_camera/pylon_camera.h | 14 ++++ include/pylon_camera/pylon_camera_parameter.h | 11 +++ src/pylon_camera/pylon_camera_node.cpp | 30 +++++--- src/pylon_camera/pylon_camera_parameter.cpp | 8 ++- test/compare_ros_and_image_timestamp.py | 25 +++++++ 10 files changed, 168 insertions(+), 9 deletions(-) create mode 100755 test/compare_ros_and_image_timestamp.py diff --git a/config/default.yaml b/config/default.yaml index cffe01c8..913b526f 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -99,6 +99,17 @@ frame_rate: 5.0 # A typical value for this upper bound is ~2000000us. # auto_exposure_upper_limit: 2000000.0 +# The fetch_camera_timestamp flag controls +# how the time stamp inside each image's header is determined. +# If the flag is set to false, the timestamp corresponds to the ROS time +# once the image was received by the host computer. +# If the flag is set to true, the time stamp from the camera, +# which contains a stamp of when the frame acquision was triggered, is fetched. +# Set this flag to true only if the camera's clock is synchronized +# with the clock of the host computer (through PTP IEEE-1588). +# By default, the flag is set to false. +# fetch_camera_timestamp: true + # The MTU size. Only used for GigE cameras. # To prevent lost frames configure the camera has to be configured # with the MTU size the network card supports. A value greater 3000 diff --git a/include/pylon_camera/internal/impl/pylon_camera_base.hpp b/include/pylon_camera/internal/impl/pylon_camera_base.hpp index 24165bb9..6f842444 100644 --- a/include/pylon_camera/internal/impl/pylon_camera_base.hpp +++ b/include/pylon_camera/internal/impl/pylon_camera_base.hpp @@ -277,6 +277,32 @@ bool PylonCameraImpl::setupSequencer(const std::vector& exp } } +template +bool PylonCameraImpl::enableTimestampChunk() +{ + // Enable chunks in general + if (GenApi::IsWritable(cam_->ChunkModeActive)) + { + cam_->ChunkModeActive.SetValue(true); + } + else + { + ROS_ERROR( "The camera doesn't support chunk features, which are needed for timestamps"); + return false; + } + // Enable time stamp chunks + cam_->ChunkSelector.SetValue(ChunkSelectorEnums::ChunkSelector_Timestamp); + cam_->ChunkEnable.SetValue(true); + /* As explained in grab(std::vector& image, ros::Time& stamp) below, + * a small GenICam node map is required for accessing chunk data each time. + * The node maps are usually created dynamically when StartGrabbing() is called. + * To avoid a delay caused by node map creation, + * we create a static pool of node maps once before grabbing. + */ + cam_->StaticChunkNodeMapPoolSize = cam_->MaxNumBuffer.GetValue(); + return true; +} + template bool PylonCameraImpl::startGrabbing(const PylonCameraParameter& parameters) { @@ -287,6 +313,10 @@ bool PylonCameraImpl::startGrabbing(const PylonCameraParameter& pa setShutterMode(parameters.shutter_mode_); } + if (parameters.fetch_camera_timestamp_) { + enableTimestampChunk(); + } + available_image_encodings_ = detectAvailableImageEncodings(); if ( !setImageEncoding(parameters.imageEncoding()) ) { @@ -341,6 +371,47 @@ bool PylonCameraImpl::grab(std::vector& image) return true; } +template +bool PylonCameraImpl::grab(std::vector& image, ros::Time& stamp) +{ + Pylon::CGrabResultPtr ptr_grab_result; + if ( !grab(ptr_grab_result) ) + { + ROS_ERROR("Error: Grab was not successful"); + return false; + } + + const uint8_t *pImageBuffer = reinterpret_cast(ptr_grab_result->GetBuffer()); + image.assign(pImageBuffer, pImageBuffer + img_size_byte_); + + /* Get pointer to the chunkTimestamp via the chunk data node map. + * To avoid delay caused by repeated dynamic road map creation, + * we create a static pool of node maps once at startup in enableTimestampChunk() above, + * as explained in Grab_ChunkImage.cpp of pylon C++ Sample code. + */ + GenApi::CIntegerPtr chunkTimestamp(ptr_grab_result->GetChunkDataNodeMap().GetNode( "ChunkTimestamp")); + if (GenApi::IsReadable(chunkTimestamp)) + { + uint64_t chunckTimestampCopy = chunkTimestamp->GetValue(); + /* chunckTimestamp is based on a counter that counts the number of clock ticks generated by the camera. + * The unit of each tick is 8ns. So, we need to multiply the variable by 8. + * Note that multiplied value will wrap around from 0 if chunckTimestamp >= 2^61, + * or roughly 26687 days after the camera setup. */ + chunckTimestampCopy <<= 3; + stamp.fromNSec(chunckTimestampCopy); + } + else + { + ROS_WARN("Error: unable to read time stamp from camera"); + return false; + } + + if ( !is_ready_ ) + is_ready_ = true; + + return true; +} + template bool PylonCameraImpl::grab(uint8_t* image) { diff --git a/include/pylon_camera/internal/impl/pylon_camera_gige.hpp b/include/pylon_camera/internal/impl/pylon_camera_gige.hpp index fe0d166b..b0c719da 100644 --- a/include/pylon_camera/internal/impl/pylon_camera_gige.hpp +++ b/include/pylon_camera/internal/impl/pylon_camera_gige.hpp @@ -52,6 +52,7 @@ struct GigECameraTrait typedef int64_t AutoTargetBrightnessValueType; typedef Basler_GigECameraParams::ShutterModeEnums ShutterModeEnums; typedef Basler_GigECamera::UserOutputSelectorEnums UserOutputSelectorEnums; + typedef Basler_GigECamera::ChunkSelectorEnums ChunkSelectorEnums; static inline AutoTargetBrightnessValueType convertBrightness(const int& value) { diff --git a/include/pylon_camera/internal/impl/pylon_camera_usb.hpp b/include/pylon_camera/internal/impl/pylon_camera_usb.hpp index 0127379d..c643288c 100644 --- a/include/pylon_camera/internal/impl/pylon_camera_usb.hpp +++ b/include/pylon_camera/internal/impl/pylon_camera_usb.hpp @@ -52,6 +52,7 @@ struct USBCameraTrait typedef double AutoTargetBrightnessValueType; typedef Basler_UsbCameraParams::ShutterModeEnums ShutterModeEnums; typedef Basler_UsbCameraParams::UserOutputSelectorEnums UserOutputSelectorEnums; + typedef Basler_UsbCameraParams::ChunkSelectorEnums ChunkSelectorEnums; static inline AutoTargetBrightnessValueType convertBrightness(const int& value) { diff --git a/include/pylon_camera/internal/pylon_camera.h b/include/pylon_camera/internal/pylon_camera.h index e65de355..1a04e793 100644 --- a/include/pylon_camera/internal/pylon_camera.h +++ b/include/pylon_camera/internal/pylon_camera.h @@ -68,6 +68,8 @@ class PylonCameraImpl : public PylonCamera virtual bool grab(uint8_t* image); + virtual bool grab(std::vector& image, ros::Time& stamp); + virtual bool setShutterMode(const pylon_camera::SHUTTER_MODE& mode); virtual bool setBinningX(const size_t& target_binning_x, @@ -143,6 +145,7 @@ class PylonCameraImpl : public PylonCamera typedef typename CameraTraitT::GainType GainType; typedef typename CameraTraitT::ShutterModeEnums ShutterModeEnums; typedef typename CameraTraitT::UserOutputSelectorEnums UserOutputSelectorEnums; + typedef typename CameraTraitT::ChunkSelectorEnums ChunkSelectorEnums; CBaslerInstantCameraT* cam_; @@ -165,6 +168,8 @@ class PylonCameraImpl : public PylonCamera virtual bool setupSequencer(const std::vector& exposure_times, std::vector& exposure_times_set); + + virtual bool enableTimestampChunk(); }; } // namespace pylon_camera diff --git a/include/pylon_camera/pylon_camera.h b/include/pylon_camera/pylon_camera.h index 36434fcc..26f3967e 100644 --- a/include/pylon_camera/pylon_camera.h +++ b/include/pylon_camera/pylon_camera.h @@ -97,6 +97,12 @@ class PylonCamera */ virtual bool applyCamSpecificStartupSettings(const PylonCameraParameter& parameters) = 0; + /** + * Enables the chunk features to grab timestamp from the camera. + * @return true if timestamp chunk is successfully enabled + */ + virtual bool enableTimestampChunk() = 0; + /** * Initializes the internal parameters of the PylonCamera instance. * @param parameters The PylonCameraParameter set to use @@ -119,6 +125,14 @@ class PylonCamera */ virtual bool grab(uint8_t* image) = 0; + /** + * Grab a camera frame with its time stamp and copy the result into image + * @param image reference to the output image + * @param stamp reference to the output time stamp + * @return true if image and time stamp were grabbed successfully. + */ + virtual bool grab(std::vector& image, ros::Time& stamp) = 0; + /** * @brief sets shutter mode for the camera (rolling or global_reset) * @param mode diff --git a/include/pylon_camera/pylon_camera_parameter.h b/include/pylon_camera/pylon_camera_parameter.h index 4644a5a8..4be05ca2 100644 --- a/include/pylon_camera/pylon_camera_parameter.h +++ b/include/pylon_camera/pylon_camera_parameter.h @@ -262,6 +262,17 @@ class PylonCameraParameter */ bool has_intrinsic_calib_; + /** + * Flag that controls how the time stamp inside each image's header is determined. + * If the flag is set to false, the timestamp corresponds to the ROS time + * once the image was received by the host computer. + * If the flag is set to true, the time stamp from the camera, + * which contains a stamp of when the frame acquision was triggered, is fetched. + * Set this flag to true only if the camera's clock is synchronized + * with the clock of the host computer (through PTP IEEE-1588). + */ + bool fetch_camera_timestamp_; + protected: /** * Validates the parameter set found on the ros parameter server. diff --git a/src/pylon_camera/pylon_camera_node.cpp b/src/pylon_camera/pylon_camera_node.cpp index ff5cd188..317df8c5 100644 --- a/src/pylon_camera/pylon_camera_node.cpp +++ b/src/pylon_camera/pylon_camera_node.cpp @@ -447,12 +447,22 @@ void PylonCameraNode::spin() bool PylonCameraNode::grabImage() { boost::lock_guard lock(grab_mutex_); - if ( !pylon_camera_->grab(img_raw_msg_.data) ) + bool grab_result; + if (pylon_camera_parameter_set_.fetch_camera_timestamp_) { - ROS_WARN("Pylon camera returned invalid image! Skipping"); + grab_result = pylon_camera_->grab(img_raw_msg_.data, img_raw_msg_.header.stamp); + } + else + { + grab_result = pylon_camera_->grab(img_raw_msg_.data); + img_raw_msg_.header.stamp = ros::Time::now(); + } + if (!grab_result) + { + // more specific error message is logged by the called function. + ROS_WARN("Error while grabbing image! Skipping"); return false; } - img_raw_msg_.header.stamp = ros::Time::now(); return true; } @@ -673,14 +683,18 @@ camera_control_msgs::GrabImagesResult PylonCameraNode::grabImagesRaw( // already contains the number of channels img.step = img.width * pylon_camera_->imagePixelDepth(); - if ( !pylon_camera_->grab(img.data) ) + if ( pylon_camera_parameter_set_.fetch_camera_timestamp_ ) { - result.success = false; - break; + result.success = pylon_camera_->grab(img.data, img.header.stamp); } - - img.header.stamp = ros::Time::now(); + else + { + result.success = pylon_camera_->grab(img.data); + img.header.stamp = ros::Time::now(); + } + if ( !result.success ) break; img.header.frame_id = cameraFrame(); + feedback.curr_nr_images_taken = i+1; if ( action_server != nullptr ) diff --git a/src/pylon_camera/pylon_camera_parameter.cpp b/src/pylon_camera/pylon_camera_parameter.cpp index 1947601b..6def6f4c 100644 --- a/src/pylon_camera/pylon_camera_parameter.cpp +++ b/src/pylon_camera/pylon_camera_parameter.cpp @@ -63,7 +63,8 @@ PylonCameraParameter::PylonCameraParameter() : auto_exp_upper_lim_(0.0), mtu_size_(3000), inter_pkg_delay_(1000), - shutter_mode_(SM_DEFAULT) + shutter_mode_(SM_DEFAULT), + fetch_camera_timestamp_(false) {} PylonCameraParameter::~PylonCameraParameter() @@ -206,6 +207,11 @@ void PylonCameraParameter::readFromRosParameterServer(const ros::NodeHandle& nh) } // ########################## + if ( nh.hasParam("fetch_camera_timestamp")) + { + nh.getParam("fetch_camera_timestamp", fetch_camera_timestamp_); + } + nh.param("exposure_search_timeout", exposure_search_timeout_, 5.); nh.param("auto_exposure_upper_limit", auto_exp_upper_lim_, 10000000.); diff --git a/test/compare_ros_and_image_timestamp.py b/test/compare_ros_and_image_timestamp.py new file mode 100755 index 00000000..d9e14f4b --- /dev/null +++ b/test/compare_ros_and_image_timestamp.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python +# simple script to test the feature to fetch camera's time stamp. +# compares ros time and camera time +import rospy +from sensor_msgs.msg import Image + +prev_ros_time = prev_img_time = None + +def callback(msg): + global prev_ros_time, prev_img_time + new_ros_time = rospy.Time.now() + new_img_time = msg.header.stamp + if prev_ros_time is not None and prev_img_time is not None: + ros_dur = new_ros_time - prev_ros_time + img_dur = new_img_time - prev_img_time + rospy.loginfo('ros time: %d seconds and %d nanoseconds', new_ros_time.secs, new_ros_time.nsecs) + rospy.loginfo('img time: %d seconds and %d nanoseconds', new_img_time.secs, new_img_time.nsecs) + rospy.loginfo('ros duration: %d seconds and %d nanoseconds', ros_dur.secs, ros_dur.nsecs) + rospy.loginfo('img duration: %d seconds and %d nanoseconds', img_dur.secs, img_dur.nsecs) + prev_ros_time = new_ros_time + prev_img_time = new_img_time + +rospy.init_node('compare_ros_and_image_timestamp') +sub = rospy.Subscriber('/pylon_camera_node/image_raw', Image, callback) +rospy.spin() From 146e62e46fd3a869620ce042f92df519c20942bf Mon Sep 17 00:00:00 2001 From: flajolet Date: Fri, 13 Apr 2018 16:51:02 -0700 Subject: [PATCH 2/2] Add a description of the fetch_camera_timestamp in README --- README.rst | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.rst b/README.rst index 5ee391e0..0195d048 100644 --- a/README.rst +++ b/README.rst @@ -87,6 +87,9 @@ All parameters are listed in the default config file: ``config/default.yaml`` - **frame_rate** The desired publisher frame rate if listening to the topics. This parameter can only be set once at start-up. Calling the GrabImages-Action can result in a higher frame rate. +- **fetch_camera_timestamp** + Boolean set at start-up to choose which of {camera, host computer} is in charge of timestamping the image. If the camera's clock is synchronized with the clock of the host computer (e.g. through PTP IEEE-1588), setting this parameter to true improves the accuracy of the timestamps as this factors out network delays and IO delays due to a high CPU load. + **Image Intensity Settings** The following settings do **NOT** have to be set. Each camera has default values which provide an automatic image adjustment resulting in valid images