RTP header extension for downstream pose

client/src/em/em_stream_client.c

@@ -14,6 +14,10 @@
 #include "gst_common.h" // for em_sample
 #include "em/em_egl.h"
 
+#include "electricmaple.pb.h"
+
+#include <openxr/openxr.h>
+
 #include "os/os_threading.h"
 
 #include <gst/app/gstappsink.h>
@@ -26,7 +30,9 @@
 #include <gst/gstmessage.h>
 #include <gst/gstsample.h>
 #include <gst/gstutils.h>
+#include <gst/rtp/gstrtpbuffer.h>
 #include <gst/video/video-frame.h>
+#include <pb_decode.h>
 
 #include <EGL/egl.h>
 #include <GLES2/gl2ext.h>
@@ -129,6 +135,8 @@ typedef enum
 } EmStreamClientProperty;
 #endif
 
+#define RTP_TWOBYTES_HDR_EXT_ID 1 // Must be in the [1,15] range
+
 // clang-format off
 #define SINK_CAPS \
     "video/x-raw(" GST_CAPS_FEATURE_MEMORY_GL_MEMORY "), "              \
@@ -261,6 +269,68 @@ em_stream_client_class_init(EmStreamClientClass *klass)
 
 #endif
 
+static inline bool
+em_stream_client_extract_frame_data(GstBuffer *buffer, em_proto_DownMessage *msg)
+{
+
+	GstRTPBuffer rtp_buffer = GST_RTP_BUFFER_INIT;
+
+	// extract Downstream metadata from rtp header
+	if (!gst_rtp_buffer_map(buffer, GST_MAP_WRITE, &rtp_buffer)) {
+		ALOGE("Failed to map GstBuffer");
+		return false;
+	}
+
+	// Not all buffers has extension data attached, check.
+	if (!gst_rtp_buffer_get_extension(&rtp_buffer)) {
+		goto no_buf;
+	}
+	uint8_t buf[em_proto_DownMessage_size] = {};
+	guint size = 0;
+	if (!gst_rtp_buffer_get_extension_twobytes_header(&rtp_buffer, NULL, RTP_TWOBYTES_HDR_EXT_ID,
+	                                                  0 /* NOTE: We do not support multi-extension-elements.*/,
+	                                                  &buf, &size)) {
+
+		ALOGE("Could not retrieve twobyte rtp extension on buffer!");
+		goto no_buf;
+	}
+	pb_istream_t our_istream = pb_istream_from_buffer(buf, size);
+
+	bool result = pb_decode_ex(&our_istream, em_proto_DownMessage_fields, msg, PB_DECODE_NULLTERMINATED);
+
+	if (!result) {
+		ALOGE("Error! %s", PB_GET_ERROR(&our_istream));
+		goto no_buf;
+	}
+
+	gst_rtp_buffer_unmap(&rtp_buffer);
+	return true;
+
+no_buf:
+	gst_rtp_buffer_unmap(&rtp_buffer);
+	return false;
+}
+
+static inline XrQuaternionf
+quat_to_openxr(const em_proto_Quaternion *q)
+{
+	return (XrQuaternionf){q->x, q->y, q->z, q->w};
+}
+static inline XrVector3f
+vec3_to_openxr(const em_proto_Vec3 *v)
+{
+	return (XrVector3f){v->x, v->y, v->z};
+}
+
+static inline XrPosef
+pose_to_openxr(const em_proto_Pose *p)
+{
+	return (XrPosef){
+	    p->has_orientation ? quat_to_openxr(&p->orientation) : (XrQuaternionf){0, 0, 0, 1},
+	    p->has_position ? vec3_to_openxr(&p->position) : (XrVector3f){0, 0, 0},
+	};
+}
+
 /*
  * callbacks
  */
@@ -346,6 +416,7 @@ on_new_sample_cb(GstAppSink *appsink, gpointer user_data)
 	GstSample *prevSample = NULL;
 	GstSample *sample = gst_app_sink_pull_sample(appsink);
 	g_assert_nonnull(sample);
+
 	{
 		g_autoptr(GMutexLocker) locker = g_mutex_locker_new(&sc->sample_mutex);
 		prevSample = sc->sample;
@@ -612,9 +683,30 @@ em_stream_client_try_pull_sample(EmStreamClient *sc, struct timespec *out_decode
 	}
 	*out_decode_end = decode_end;
 
+	struct em_sc_sample *ret = calloc(1, sizeof(struct em_sc_sample));
+
 	// ALOGE("FRED: GOT A SAMPLE !!!");
 	GstBuffer *buffer = gst_sample_get_buffer(sample);
 	GstCaps *caps = gst_sample_get_caps(sample);
+	GstRTPBuffer rtp_buffer = GST_RTP_BUFFER_INIT;
+
+	// extract Downstream metadata from rtp header
+	em_proto_DownMessage msg = em_proto_DownMessage_init_default;
+	if (em_stream_client_extract_frame_data(buffer, &msg)) {
+		ALOGI("RYLIE: got downstream frame message");
+	}
+	if (msg.has_frame_data && msg.frame_data.has_P_localSpace_view0 && msg.frame_data.has_P_localSpace_view1) {
+		// OK we have a message for this one.
+		ALOGI("RYLIE: got downstream frame message with poses!");
+		// TODO is it too late to get it here?
+		ret->base.have_poses = true;
+		ret->base.poses[0] = pose_to_openxr(&msg.frame_data.P_localSpace_view0);
+		ret->base.poses[1] = pose_to_openxr(&msg.frame_data.P_localSpace_view1);
+
+		// TODO: use msg.frame_id (and others) and populate properly inside stream client.
+		// ...
+		// ...
+	}
 
 	GstVideoInfo info;
 	gst_video_info_from_caps(&info, caps);
@@ -630,8 +722,6 @@ em_stream_client_try_pull_sample(EmStreamClient *sc, struct timespec *out_decode
 	}
 #endif
 
-	struct em_sc_sample *ret = calloc(1, sizeof(struct em_sc_sample));
-
 	GstVideoFrame frame;
 	GstMapFlags flags = (GstMapFlags)(GST_MAP_READ | GST_MAP_GL);
 	gst_video_frame_map(&frame, &info, buffer, flags);

client/src/em/gst_common.h

@@ -36,4 +36,7 @@ struct em_sample
 {
 	GLuint frame_texture_id;
 	GLenum frame_texture_target;
+
+	bool have_poses;
+	XrPosef poses[2];
 };

proto/electricmaple.proto

@@ -101,8 +101,9 @@ message UpMessage {
 
 message DownFrameDataMessage {
 	int64 frame_sequence_id = 1;
-	Pose P_localSpace_viewSpace = 2;
-	int64 display_time = 3;
+	Pose P_localSpace_view0 = 2; // Left view
+	Pose P_localSpace_view1 = 3; // Right view
+	int64 display_time = 4;
 	// TODO fovs here
 }
 

proto/generated/electricmaple.pb.h

@@ -127,8 +127,10 @@ typedef struct _em_proto_UpMessage {
 
 typedef struct _em_proto_DownFrameDataMessage {
     int64_t frame_sequence_id;
-    bool has_P_localSpace_viewSpace;
-    em_proto_Pose P_localSpace_viewSpace;
+    bool has_P_localSpace_view0;
+    em_proto_Pose P_localSpace_view0; /* Left view */
+    bool has_P_localSpace_view1;
+    em_proto_Pose P_localSpace_view1; /* Right view */
     int64_t display_time; /* TODO fovs here */
 } em_proto_DownFrameDataMessage;
 
@@ -177,7 +179,7 @@ extern "C" {
 #define em_proto_TouchControllerRight_init_default {false, em_proto_InputClickTouch_init_default, false, em_proto_InputClickTouch_init_default, false, em_proto_InputClickTouch_init_default, false, em_proto_TouchControllerCommon_init_default}
 #define em_proto_UpFrameMessage_init_default     {0, 0, 0, 0}
 #define em_proto_UpMessage_init_default          {0, false, em_proto_TrackingMessage_init_default, false, em_proto_UpFrameMessage_init_default}
-#define em_proto_DownFrameDataMessage_init_default {0, false, em_proto_Pose_init_default, 0}
+#define em_proto_DownFrameDataMessage_init_default {0, false, em_proto_Pose_init_default, false, em_proto_Pose_init_default, 0}
 #define em_proto_DownMessage_init_default        {false, em_proto_DownFrameDataMessage_init_default}
 #define em_proto_Quaternion_init_zero            {0, 0, 0, 0}
 #define em_proto_Vec3_init_zero                  {0, 0, 0}
@@ -192,7 +194,7 @@ extern "C" {
 #define em_proto_TouchControllerRight_init_zero  {false, em_proto_InputClickTouch_init_zero, false, em_proto_InputClickTouch_init_zero, false, em_proto_InputClickTouch_init_zero, false, em_proto_TouchControllerCommon_init_zero}
 #define em_proto_UpFrameMessage_init_zero        {0, 0, 0, 0}
 #define em_proto_UpMessage_init_zero             {0, false, em_proto_TrackingMessage_init_zero, false, em_proto_UpFrameMessage_init_zero}
-#define em_proto_DownFrameDataMessage_init_zero  {0, false, em_proto_Pose_init_zero, 0}
+#define em_proto_DownFrameDataMessage_init_zero  {0, false, em_proto_Pose_init_zero, false, em_proto_Pose_init_zero, 0}
 #define em_proto_DownMessage_init_zero           {false, em_proto_DownFrameDataMessage_init_zero}
 
 /* Field tags (for use in manual encoding/decoding) */
@@ -243,8 +245,9 @@ extern "C" {
 #define em_proto_UpMessage_tracking_tag          2
 #define em_proto_UpMessage_frame_tag             3
 #define em_proto_DownFrameDataMessage_frame_sequence_id_tag 1
-#define em_proto_DownFrameDataMessage_P_localSpace_viewSpace_tag 2
-#define em_proto_DownFrameDataMessage_display_time_tag 3
+#define em_proto_DownFrameDataMessage_P_localSpace_view0_tag 2
+#define em_proto_DownFrameDataMessage_P_localSpace_view1_tag 3
+#define em_proto_DownFrameDataMessage_display_time_tag 4
 #define em_proto_DownMessage_frame_data_tag      1
 
 /* Struct field encoding specification for nanopb */
@@ -371,11 +374,13 @@ X(a, STATIC,   OPTIONAL, MESSAGE,  frame,             3)
 
 #define em_proto_DownFrameDataMessage_FIELDLIST(X, a) \
 X(a, STATIC,   SINGULAR, INT64,    frame_sequence_id,   1) \
-X(a, STATIC,   OPTIONAL, MESSAGE,  P_localSpace_viewSpace,   2) \
-X(a, STATIC,   SINGULAR, INT64,    display_time,      3)
+X(a, STATIC,   OPTIONAL, MESSAGE,  P_localSpace_view0,   2) \
+X(a, STATIC,   OPTIONAL, MESSAGE,  P_localSpace_view1,   3) \
+X(a, STATIC,   SINGULAR, INT64,    display_time,      4)
 #define em_proto_DownFrameDataMessage_CALLBACK NULL
 #define em_proto_DownFrameDataMessage_DEFAULT NULL
-#define em_proto_DownFrameDataMessage_P_localSpace_viewSpace_MSGTYPE em_proto_Pose
+#define em_proto_DownFrameDataMessage_P_localSpace_view0_MSGTYPE em_proto_Pose
+#define em_proto_DownFrameDataMessage_P_localSpace_view1_MSGTYPE em_proto_Pose
 
 #define em_proto_DownMessage_FIELDLIST(X, a) \
 X(a, STATIC,   OPTIONAL, MESSAGE,  frame_data,        1)
@@ -417,8 +422,8 @@ extern const pb_msgdesc_t em_proto_DownMessage_msg;
 #define em_proto_DownMessage_fields &em_proto_DownMessage_msg
 
 /* Maximum encoded size of messages (where known) */
-#define em_proto_DownFrameDataMessage_size       63
-#define em_proto_DownMessage_size                65
+#define em_proto_DownFrameDataMessage_size       104
+#define em_proto_DownMessage_size                106
 #define em_proto_InputClickTouch_size            4
 #define em_proto_InputThumbstick_size            16
 #define em_proto_InputValueTouch_size            7

server/CMakeLists.txt

@@ -44,6 +44,7 @@ find_package(PkgConfig REQUIRED)
 # GLib packages we'll need
 pkg_check_modules(GLIB REQUIRED glib-2.0)
 pkg_check_modules(GST_SDP REQUIRED gstreamer-sdp-1.0)
+pkg_check_modules(GST_RTP REQUIRED gstreamer-rtp-1.0)
 pkg_check_modules(GST_WEBRTC REQUIRED gstreamer-webrtc-1.0)
 pkg_check_modules(GST REQUIRED gstreamer-plugins-base-1.0)
 pkg_check_modules(GST REQUIRED gstreamer-plugins-bad-1.0)

server/src/ems/CMakeLists.txt

@@ -27,6 +27,7 @@ target_link_libraries(
 		comp_util
 		comp_multi
 		ems_gst
+		em_proto
 	)
 target_include_directories(comp_ems PUBLIC . ${GST_INCLUDE_DIRS})
 

server/src/ems/ems_compositor.cpp

@@ -17,6 +17,9 @@
 
 #include "ems_compositor.h"
 
+
+#include "electricmaple.pb.h"
+
 #include "gstreamer/gst_internal.h"
 #include "os/os_time.h"
 
@@ -35,6 +38,7 @@
 #include "vk/vk_cmd.h"
 #include "vk/vk_cmd_pool.h"
 
+
 #include <stdio.h>
 #include <stdarg.h>
 
@@ -333,6 +337,16 @@ compositor_init_sys_info(struct ems_compositor *c, struct xrt_device *xdev)
 	return true;
 }
 
+static inline em_proto_Pose
+to_proto(const struct xrt_pose &pose)
+{
+	em_proto_Pose ret = em_proto_Pose_init_default;
+	ret.has_position = true;
+	ret.position = {pose.position.x, pose.position.y, pose.position.z};
+	ret.has_orientation = true;
+	ret.orientation = {pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z};
+	return ret;
+}
 
 /*
  *
@@ -508,7 +522,20 @@ pack_blit_and_encode(struct ems_compositor *c,
 	wrap->base_frame.source_timestamp = wrap->base_frame.timestamp;
 	wrap->base_frame.source_sequence = c->image_sequence++;
 	wrap->base_frame.source_id = 0;
-	wrap = NULL;
+
+	// set the latest Downstream mesg before pushing the frame
+	em_proto_DownMessage msg = em_proto_DownMessage_init_default;
+	msg.has_frame_data = true;
+	msg.frame_data.frame_sequence_id = wrap->base_frame.source_sequence;
+	msg.frame_data.has_P_localSpace_view0 = true;
+	msg.frame_data.P_localSpace_view0 = to_proto(lvd->pose);
+	msg.frame_data.has_P_localSpace_view1 = true;
+	msg.frame_data.P_localSpace_view1 = to_proto(rvd->pose);
+	// msg.frame_datadisplay_time; /* Needed ?*/
+
+	wrap = NULL; // important to keep this line after setting "msg.frame_sequence_id" above.
+
+	ems_gstreamer_pipeline_set_down_msg(c->gstreamer_pipeline, &msg);
 
 	if (!c->pipeline_playing) {
 		ems_gstreamer_pipeline_play(c->gstreamer_pipeline);

server/src/ems/gst/CMakeLists.txt

@@ -14,6 +14,7 @@ target_link_libraries(
 		aux_gstreamer
 		${GST_LIBRARIES}
 		${GST_SDP_LIBRARIES}
+		${GST_RTP_LIBRARIES}
 		${GST_WEBRTC_LIBRARIES}
 		${GLIB_LIBRARIES}
 		${LIBSOUP_LIBRARIES}