Fetch the cameras's timestamp associated with an image

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

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

include/pylon_camera/internal/impl/pylon_camera_base.hpp

@@ -277,6 +277,32 @@ bool PylonCameraImpl<CameraTraitT>::setupSequencer(const std::vector<float>& exp
     }
 }
 
+template <typename CameraTraitT>
+bool PylonCameraImpl<CameraTraitT>::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<uint8_t>& 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 <typename CameraTraitT>
 bool PylonCameraImpl<CameraTraitT>::startGrabbing(const PylonCameraParameter& parameters)
 {
@@ -287,6 +313,10 @@ bool PylonCameraImpl<CameraTraitT>::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<CameraTrait>::grab(std::vector<uint8_t>& image)
     return true;
 }
 
+template <typename CameraTrait>
+bool PylonCameraImpl<CameraTrait>::grab(std::vector<uint8_t>& 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<uint8_t*>(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 <typename CameraTrait>
 bool PylonCameraImpl<CameraTrait>::grab(uint8_t* image)
 {

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

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

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<uint8_t>& 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<float>& exposure_times,
                                 std::vector<float>& exposure_times_set);
+
+    virtual bool enableTimestampChunk();
 };
 
 }  // namespace pylon_camera

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<uint8_t>& image, ros::Time& stamp) = 0;
+
     /**
      * @brief sets shutter mode for the camera (rolling or global_reset)
      * @param mode

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.

src/pylon_camera/pylon_camera_node.cpp

@@ -447,12 +447,22 @@ void PylonCameraNode::spin()
 bool PylonCameraNode::grabImage()
 {
     boost::lock_guard<boost::recursive_mutex> 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 )

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<double>("exposure_search_timeout", exposure_search_timeout_, 5.);
     nh.param<double>("auto_exposure_upper_limit", auto_exp_upper_lim_, 10000000.);
 

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