feat: add manual time offset to rosimagesink image publisher

gst_bridge/include/gst_bridge/rosbasesink.h

@@ -43,6 +43,7 @@ struct _RosBaseSink
   GstBaseSink parent;
   gchar* node_name;
   gchar* node_namespace;
+  gint64 offset_time_ns;
 
   rclcpp::Context::SharedPtr ros_context;
   rclcpp::Executor::SharedPtr ros_executor;

gst_bridge/include/gst_bridge/rosimagesink.h

@@ -26,6 +26,7 @@
 
 //include ROS and ROS message formats
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/duration.hpp>
 #include <sensor_msgs/msg/image.hpp>
 
 

gst_bridge/src/rosbasesink.cpp

@@ -64,6 +64,7 @@ enum
   PROP_ROS_NAME,
   PROP_ROS_NAMESPACE,
   PROP_ROS_START_TIME,
+  PROP_ROS_TIME_OFFSET,
 };
 
 
@@ -107,6 +108,12 @@ static void rosbasesink_class_init (RosBaseSinkClass * klass)
       (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS))
   );
 
+  g_object_class_install_property (object_class, PROP_ROS_TIME_OFFSET,
+      g_param_spec_int64 ("ros-time-offset", "ros-time-offset", "ROS time offset (nanoseconds)",
+      G_MININT64, G_MAXINT64, GST_CLOCK_TIME_NONE,
+      (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS))
+  );
+
   element_class->change_state = GST_DEBUG_FUNCPTR (rosbasesink_change_state); //use state change events to open and close publishers
   basesink_class->render = GST_DEBUG_FUNCPTR (rosbasesink_render); // gives us a buffer to forward
 
@@ -117,6 +124,7 @@ static void rosbasesink_init (RosBaseSink * sink)
   sink->node_name = g_strdup("gst_base_sink_node");
   sink->node_namespace = g_strdup("");
   sink->stream_start_prop = GST_CLOCK_TIME_NONE;
+  sink->offset_time_ns = 0;
 }
 
 void rosbasesink_set_property (GObject * object, guint property_id,
@@ -162,6 +170,17 @@ void rosbasesink_set_property (GObject * object, guint property_id,
       }
       break;
 
+    case PROP_ROS_TIME_OFFSET:
+      if(sink->node)
+      {
+        RCLCPP_ERROR(sink->logger, "can't change time_offset once opened");
+      }
+      else
+      {
+        sink->offset_time_ns = g_value_get_int64(value);
+      }
+      break;
+
     default:
       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, property_id, pspec);
       break;
@@ -188,6 +207,10 @@ void rosbasesink_get_property (GObject * object, guint property_id,
       // XXX this allows inspection via props,
       //      but may cause confusion because it does not show the actual prop
       break;
+
+    case PROP_ROS_TIME_OFFSET:
+      g_value_set_int64(value, sink->offset_time_ns);
+      break;
 
     default:
       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, property_id, pspec);

gst_bridge/src/rosbasesrc.cpp

@@ -64,6 +64,7 @@ enum
   PROP_ROS_NAME,
   PROP_ROS_NAMESPACE,
   PROP_ROS_START_TIME,
+  PROP_ROS_TIME_OFFSET,
 };
 
 /* class initialization */

gst_bridge/src/rosimagesink.cpp

@@ -37,6 +37,7 @@
 #include <gst_bridge/rosimagesink.h>
 
 
+
 GST_DEBUG_CATEGORY_STATIC (rosimagesink_debug_category);
 #define GST_CAT_DEFAULT rosimagesink_debug_category
 
@@ -310,6 +311,37 @@ static GstFlowReturn rosimagesink_render (RosBaseSink * ros_base_sink, GstBuffer
   Rosimagesink *sink = GST_ROSIMAGESINK (ros_base_sink);
   GST_DEBUG_OBJECT (sink, "render");
 
+  int32_t sec = 0;
+  uint32_t nsec = 0;
+
+  // Make duration an absolute value due to duration object definition
+  if (abs(ros_base_sink->offset_time_ns) > G_MAXUINT32)
+  {
+    //need to to convert value to a integer value of seconds while leaving the rest in nanosecs to maintain precision
+    //as nsec of Duration is an uint32 object.
+    RCLCPP_DEBUG(ros_base_sink->logger, "time offset is greater than 32 bits");
+    sec = static_cast<int32_t>(abs(ros_base_sink->offset_time_ns) / G_GINT64_CONSTANT(1000000000));
+    nsec = static_cast<uint32_t>(abs(ros_base_sink->offset_time_ns) - (sec * G_GINT64_CONSTANT(1000000000)));
+  }
+  else
+  {
+    sec = 0;
+    nsec = static_cast<uint32_t>(abs(ros_base_sink->offset_time_ns));
+  }
+
+  rclcpp::Duration offset_time(sec, nsec);
+
+  if (ros_base_sink->offset_time_ns < 0)
+  {
+    RCLCPP_DEBUG(ros_base_sink->logger, "time offset is negative %f seconds or %ld nanosecs", offset_time.seconds(), offset_time.nanoseconds());
+    msg_time = msg_time - offset_time;
+  }
+  else
+  {
+    RCLCPP_DEBUG(ros_base_sink->logger, "time offset is positive %f seconds or %ld nanosecs", offset_time.seconds(), offset_time.nanoseconds());
+    msg_time = msg_time + offset_time;
+  }
+
   msg.header.stamp = msg_time;
   msg.header.frame_id = sink->frame_id;