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 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()