Merge pull request #272 from jaimesx/timestamp-fix

Fixed timestamps
ros-drivers · Aug 31, 2023 · ef97b79 · ef97b79
2 parents 7b480fd + db11807
commit ef97b79
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 32 deletions.
diff --git a/include/usb_cam/camera_driver.h b/include/usb_cam/camera_driver.h
@@ -45,7 +45,7 @@ class AbstractV4LUSBCam
     static color_format_t color_format;
     static bool monochrome;
     static int file_dev; // fd_
-    static const time_t epoch_time_shift;
+    static const time_t epoch_time_shift_us;
 
     /* FFMPEG */
     static bool full_ffmpeg_log;

diff --git a/include/usb_cam/util.h b/include/usb_cam/util.h
@@ -13,7 +13,13 @@ namespace util
 /// @brief Get epoch time shift
 /// @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
-time_t get_epoch_time_shift();
+time_t get_epoch_time_shift_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.
+timespec calc_img_timestamp(const timeval & buffer_time, const time_t & epoch_time_shift_us);
 
 int xioctl(int fd, int request, void * arg);
 

diff --git a/src/camera_driver.cpp b/src/camera_driver.cpp
@@ -16,7 +16,7 @@ pixel_format_t AbstractV4LUSBCam::pixel_format = PIXEL_FORMAT_UNKNOWN;
 color_format_t AbstractV4LUSBCam::color_format = COLOR_FORMAT_UNKNOWN;
 bool AbstractV4LUSBCam::monochrome = false;
 int AbstractV4LUSBCam::file_dev = -1;
-const time_t AbstractV4LUSBCam::epoch_time_shift = util::get_epoch_time_shift();
+const time_t AbstractV4LUSBCam::epoch_time_shift_us = util::get_epoch_time_shift_us();
 
 /* FFMPEG */
 bool AbstractV4LUSBCam::full_ffmpeg_log = false;
@@ -527,7 +527,7 @@ camera_image_t *AbstractV4LUSBCam::read_frame()
     unsigned int i;
     int len;
     struct timespec stamp;
-    int64_t buffer_time_s;
+    int64_t buffer_time_us;
     switch(io_method)
     {
     case IO_METHOD_READ:
@@ -560,18 +560,16 @@ camera_image_t *AbstractV4LUSBCam::read_frame()
                 return nullptr;
             }
         }
-        buffer_time_s = buf.timestamp.tv_sec + static_cast<int64_t>(round(buf.timestamp.tv_usec / 1000000.0));
-        stamp.tv_sec = static_cast<time_t>(round(buffer_time_s)) + epoch_time_shift;
-        stamp.tv_nsec = static_cast<int64_t>(buf.timestamp.tv_usec * 1000.0);
+        image->stamp = util::calc_img_timestamp(buf.timestamp, epoch_time_shift_us);
+        timespec_get(&image->stamp, TIME_UTC);
         assert(buf.index < buffers_count);
         len = buf.bytesused;
         // Process image
         if(usb_cam::util::xioctl(file_dev, static_cast<int>(VIDIOC_QBUF), &buf) < 0)
         {
             printf("Unable to exchange buffer with driver (%i)\n", errno);
             return nullptr;
-        }
-        image->stamp = stamp;
+        } 
         break;
     case IO_METHOD_USERPTR:
         CLEAR(buf);
@@ -588,10 +586,8 @@ camera_image_t *AbstractV4LUSBCam::read_frame()
                 return nullptr;
             }
         }
-        buffer_time_s = buf.timestamp.tv_sec + static_cast<int64_t>(round(buf.timestamp.tv_usec / 1000000.0));
-
-        stamp.tv_sec = static_cast<time_t>(round(buffer_time_s)) + epoch_time_shift;
-        stamp.tv_nsec = static_cast<int64_t>(buf.timestamp.tv_usec / 1000.0);
+        image->stamp = util::calc_img_timestamp(buf.timestamp, epoch_time_shift_us);
+        timespec_get(&image->stamp, TIME_UTC);
 
         for(i = 0; i < buffers_count; ++i)
             if(buf.m.userptr == reinterpret_cast<uint64_t>(buffers[i].start) && buf.length == buffers[i].length)
@@ -603,8 +599,7 @@ camera_image_t *AbstractV4LUSBCam::read_frame()
         {
             printf("Unable to exchange buffer with driver (%i)\n", errno);
             return nullptr;
-        }
-        image->stamp = stamp;
+        } 
         break;
     default:
         printf("Attempt to grab the frame via unknown I/O method (%i)\n", errno);

diff --git a/src/util.cpp b/src/util.cpp
@@ -2,7 +2,7 @@
 
 using namespace usb_cam;
 
-time_t get_epoch_time_shift()
+time_t util::get_epoch_time_shift_us()
 {
     struct timeval epoch_time;
     struct timespec monotonic_time;
@@ -20,6 +20,19 @@ time_t get_epoch_time_shift()
     return static_cast<time_t>((epoch_ms - uptime_ms) / 1000);
 }
 
+timespec util::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;
+}
+
 int util::xioctl(int fd, int request, void * arg)
 {
     int r;
@@ -47,19 +60,4 @@ unsigned char util::CLIPVALUE(const int & val)
         unsigned char clipped_val = val < 0 ? 0 : static_cast<unsigned char>(val);
         return val > 255 ? 255 : clipped_val;
     }
-}
-
-time_t util::get_epoch_time_shift()
-{
-    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));
-    return static_cast<time_t>((epoch_ms - uptime_ms) / 1000);
-}
+}