fix: ros2 gstreamer timestamps (#83)

* fix: correct gstreamer timestamp offset calculation for image header timestamp

* docs: add use_gst_timestamps parameter to the readme file

README.md

@@ -60,6 +60,7 @@ Note that GStreamer is licensed under the LGPL, and GStreamer plugins have their
 * `frame_id`: The [tf2](https://index.ros.org/p/tf2/) frame ID
 * `reopen_on_eof`: Re-open the stream if it ends (EOF)
 * `sync_sink`: Synchronize the app sink (sometimes setting this to `false` can resolve problems with sub-par framerates)
+* `use_gst_timestamps`: Use the GStreamer buffer timestamps for the image message header timestamps (setting this to `false` results in header timestamps being the time that the image buffer transfer is completed)
 * `image_encoding`: image encoding ("rgb8", "mono8", "yuv422", "jpeg")
 * `use_sensor_data_qos`: The flag to use sensor data qos for camera topic(image, camera_info)
 

include/gscam/gscam.hpp

@@ -73,7 +73,7 @@ class GSCam : public rclcpp::Node
 
   // ROS Inteface
   // Calibration between ros::Time and gst timestamps
-  double time_offset_;
+  uint64_t time_offset_;
   camera_info_manager::CameraInfoManager camera_info_manager_;
   image_transport::CameraPublisher camera_pub_;
   // Case of a jpeg only publisher

src/gscam.cpp

@@ -236,8 +236,8 @@ bool GSCam::init_stream()
   GstClock * clock = gst_system_clock_obtain();
   GstClockTime ct = gst_clock_get_time(clock);
   gst_object_unref(clock);
-  time_offset_ = now().seconds() - GST_TIME_AS_USECONDS(ct) / 1e6;
-  RCLCPP_INFO(get_logger(), "Time offset: %.3f", time_offset_);
+  time_offset_ = now().nanoseconds() - GST_TIME_AS_NSECONDS(ct);
+  RCLCPP_INFO(get_logger(), "Time offset: %.6f", rclcpp::Time(time_offset_).seconds());
 
   gst_element_set_state(pipeline_, GST_STATE_PAUSED);
 
@@ -353,7 +353,7 @@ void GSCam::publish_stream()
     sensor_msgs::msg::CameraInfo::SharedPtr cinfo;
     cinfo.reset(new sensor_msgs::msg::CameraInfo(cur_cinfo));
     if (use_gst_timestamps_) {
-      cinfo->header.stamp = rclcpp::Time(GST_TIME_AS_USECONDS(buf->pts + bt) / 1e6 + time_offset_);
+      cinfo->header.stamp = rclcpp::Time(GST_TIME_AS_NSECONDS(buf->pts + bt) + time_offset_);
     } else {
       cinfo->header.stamp = now();
     }