Merge pull request #267 from boitumeloruf/ros2

Fixed wrong image timestamp due to missing handling of microseconds in epoch time shift

include/usb_cam/usb_cam.hpp

@@ -256,9 +256,9 @@ class UsbCam
     return m_is_capturing;
   }
 
-  inline time_t get_epoch_time_shift()
+  inline time_t get_epoch_time_shift_us()
   {
-    return m_epoch_time_shift;
+    return m_epoch_time_shift_us;
   }
 
   inline std::vector<capture_format_t> supported_formats()
@@ -347,10 +347,10 @@ class UsbCam
   AVDictionary * m_avoptions;
   AVCodecContext * m_avcodec_context;
 
-  int64_t m_buffer_time_s;
+  int64_t m_buffer_time_us;
   bool m_is_capturing;
   int m_framerate;
-  const time_t m_epoch_time_shift;
+  const time_t m_epoch_time_shift_us;
   std::vector<capture_format_t> m_supported_formats;
 };
 

include/usb_cam/utils.hpp

@@ -65,27 +65,42 @@ struct buffer
 };
 
 
-/// @brief Get epoch time shift
+/// @brief Get epoch time shift in microseconds
 /// @details Run this at start of process to calculate epoch time shift
 /// @ref https://stackoverflow.com/questions/10266451/where-does-v4l2-buffer-timestamp-value-starts-counting
-inline time_t get_epoch_time_shift()
+inline time_t get_epoch_time_shift_us()
 {
   struct timeval epoch_time;
   struct timespec monotonic_time;
 
   gettimeofday(&epoch_time, NULL);
   clock_gettime(CLOCK_MONOTONIC, &monotonic_time);
 
-  const int64_t uptime_ms =
-    monotonic_time.tv_sec * 1000 + static_cast<int64_t>(
-    std::round(monotonic_time.tv_nsec / 1000000.0));
-  const int64_t epoch_ms =
-    epoch_time.tv_sec * 1000 + static_cast<int64_t>(
-    std::round(epoch_time.tv_usec / 1000.0));
+  const int64_t uptime_us =
+    monotonic_time.tv_sec * 1000000 + static_cast<int64_t>(
+    std::round(monotonic_time.tv_nsec / 1000.0));
+  const int64_t epoch_us =
+    epoch_time.tv_sec * 1000000 + epoch_time.tv_usec / 1000.0;
 
-  return static_cast<time_t>((epoch_ms - uptime_ms) / 1000);
+  return static_cast<time_t>(epoch_us - uptime_us);
 }
 
+/// @brief Calculate image timestamp from buffer time and epoch time shift.
+/// In this, the buffer time is first converted into microseconds before the epoch time shift,
+/// which is to be given in microseconds is added to it. Afterwards it is split into seconds
+/// and nanoseconds for the image timestamp.
+inline timespec calc_img_timestamp(const timeval & buffer_time, const time_t & epoch_time_shift_us)
+{
+  timespec img_timestamp;
+
+  int64_t buffer_time_us = (buffer_time.tv_sec * 1000000) + buffer_time.tv_usec;
+  buffer_time_us += epoch_time_shift_us;
+
+  img_timestamp.tv_sec = (buffer_time_us / 1000000);
+  img_timestamp.tv_nsec = (buffer_time_us % 1000000) * 1000;
+
+  return img_timestamp;
+}
 
 inline int xioctl(int fd, uint64_t request, void * arg)
 {

src/usb_cam.cpp

@@ -63,7 +63,7 @@ UsbCam::UsbCam()
   m_avframe(NULL), m_avcodec(NULL), m_avoptions(NULL),
   m_avcodec_context(NULL),
   m_is_capturing(false), m_framerate(0),
-  m_epoch_time_shift(usb_cam::utils::get_epoch_time_shift()), m_supported_formats()
+  m_epoch_time_shift_us(usb_cam::utils::get_epoch_time_shift_us()), m_supported_formats()
 {}
 
 UsbCam::~UsbCam()
@@ -133,11 +133,7 @@ void UsbCam::read_frame()
       }
 
       // Get timestamp from V4L2 image buffer
-      m_buffer_time_s =
-        buf.timestamp.tv_sec + static_cast<int64_t>(round(buf.timestamp.tv_usec / 1000000.0));
-
-      m_image.stamp.tv_sec = static_cast<time_t>(round(m_buffer_time_s)) + m_epoch_time_shift;
-      m_image.stamp.tv_nsec = static_cast<int64_t>(buf.timestamp.tv_usec * 1000.0);
+      m_image.stamp = usb_cam::utils::calc_img_timestamp(buf.timestamp, m_epoch_time_shift_us);
 
       assert(buf.index < m_number_of_buffers);
       process_image(m_buffers[buf.index].start, m_image.data, buf.bytesused);
@@ -162,11 +158,8 @@ void UsbCam::read_frame()
         }
       }
 
-      m_buffer_time_s =
-        buf.timestamp.tv_sec + static_cast<int64_t>(round(buf.timestamp.tv_usec / 1000000.0));
-
-      m_image.stamp.tv_sec = static_cast<time_t>(round(m_buffer_time_s)) + m_epoch_time_shift;
-      m_image.stamp.tv_nsec = static_cast<int64_t>(buf.timestamp.tv_usec / 1000.0);
+      // Get timestamp from V4L2 image buffer
+      m_image.stamp = usb_cam::utils::calc_img_timestamp(buf.timestamp, m_epoch_time_shift_us);
 
       for (i = 0; i < m_number_of_buffers; ++i) {
         if (buf.m.userptr == reinterpret_cast<uint64_t>(m_buffers[i].start) && \

test/test_usb_cam_utils.cpp

@@ -77,7 +77,7 @@ TEST(test_usb_cam_utils, test_clip_value) {
 TEST(test_usb_cam_utils, test_monotonic_to_real_time) {
   // Get timeval to use for test
 
-  const time_t test_time_t = usb_cam::utils::get_epoch_time_shift();
+  const time_t test_time_t = usb_cam::utils::get_epoch_time_shift_us();
 
   EXPECT_NE(test_time_t, 0);
 }