Change timestamp source based on issue anqixu#37

Author: anuppari

include/ueye_cam/ueye_cam_driver.hpp

@@ -384,6 +384,11 @@ class UEyeCamDriver {
    * Sets a timestamp indicating the moment of the image capture
    */
   bool getTimestamp(UEYETIME *timestamp);
+  
+  /**
+   * Sets a clock tick indicating the moment of the image capture
+   */
+  bool getClockTick(uint64_t *tick);
 
 
 protected:

include/ueye_cam/ueye_cam_nodelet.hpp

@@ -162,6 +162,11 @@ class UEyeCamNodelet : public nodelet::Nodelet, public UEyeCamDriver {
    * Returns image's timestamp or current wall time if driver call fails.
    */
   ros::Time getImageTimestamp();
+  
+  /**
+   * Returns image's timestamp based on device's internal clock or current wall time if driver call fails.
+   */
+  ros::Time getImageTickTimestamp();
 
   std::thread frame_grab_thread_;
   bool frame_grab_alive_;
@@ -182,6 +187,9 @@ class UEyeCamNodelet : public nodelet::Nodelet, public UEyeCamDriver {
   std::string cam_intr_filename_;
   std::string cam_params_filename_; // should be valid UEye INI file
   ueye_cam::UEyeCamConfig cam_params_;
+  
+  ros::Time init_ros_time_;
+  uint64_t init_clock_tick_;
 };
 
 

src/ueye_cam_driver.cpp

@@ -1293,4 +1293,13 @@ bool UEyeCamDriver::getTimestamp(UEYETIME *timestamp) {
   return false;
 }
 
+bool UEyeCamDriver::getClockTick(uint64_t *tick) {
+  UEYEIMAGEINFO ImageInfo;
+  if(is_GetImageInfo (cam_handle_, cam_buffer_id_, &ImageInfo, sizeof (ImageInfo)) == IS_SUCCESS) {
+    *tick = ImageInfo.u64TimestampDevice;
+    return true;
+  }
+  return false;
+}
+
 } // namespace ueye_cam

src/ueye_cam_nodelet.cpp

@@ -949,7 +949,16 @@ void UEyeCamNodelet::frameGrabLoop() {
       INT eventTimeout = (cam_params_.auto_frame_rate || cam_params_.ext_trigger_mode) ?
           (INT) 2000 : (INT) (1000.0 / cam_params_.frame_rate * 2);
       if (processNextFrame(eventTimeout) != NULL) {
-        ros_image_.header.stamp = ros_cam_info_.header.stamp = getImageTimestamp();
+        if (init_ros_time_.isZero()) {
+          if(getClockTick(&init_clock_tick_)) {
+            init_ros_time_ = getImageTimestamp();
+            
+            // Deal with instability in getImageTimestamp due to daylight savings time
+            if (abs((ros::Time::now() - init_ros_time_).toSec()) > abs((ros::Time::now() - (init_ros_time_+ros::Duration(3600,0))).toSec())) { init_ros_time_ += ros::Duration(3600,0); }
+            if (abs((ros::Time::now() - init_ros_time_).toSec()) > abs((ros::Time::now() - (init_ros_time_-ros::Duration(3600,0))).toSec())) { init_ros_time_ -= ros::Duration(3600,0); }
+          }
+        }
+        ros_image_.header.stamp = ros_cam_info_.header.stamp = getImageTickTimestamp();
         // Process new frame
 #ifdef DEBUG_PRINTOUT_FRAME_GRAB_RATES
         grabbedFrameCount++;
@@ -1069,6 +1078,14 @@ ros::Time UEyeCamNodelet::getImageTimestamp() {
   }
   return ros::Time::now();
 }
+
+ros::Time UEyeCamNodelet::getImageTickTimestamp() {
+  uint64_t tick;
+  if(getClockTick(&tick)) {
+    return init_ros_time_ + ros::Duration(double(tick - init_clock_tick_)*1e-7);
+  }
+  return ros::Time::now();
+}
 // TODO: 0 bug where nodelet locks and requires SIGTERM when there are still subscribers (need to find where does code hang)
 
 } // namespace ueye_cam