gstreamer/ext/openni2/gstopenni2src.cpp
2020-03-15 10:47:02 +00:00

757 lines
23 KiB
C++

/*
* GStreamer OpenNI2 device source element
* Copyright (C) 2013 Miguel Casas-Sanchez <miguelecasassanchez@gmail.com>
* This library is free software; you can
* redistribute it and/or modify it under the terms of the GNU Library
* General Public License as published by the Free Software Foundation;
* either version 2 of the License, or (at your option) any later version.
* This library is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Library
* General Public License for more details. You should have received a copy
* of the GNU Library General Public License along with this library; if
* not, write to the Free Software Foundation, Inc., 51 Franklin St,
* Fifth Floor, Boston, MA 02110-1301, USA.
*/
/**
* SECTION:element-openni2src
*
* ## Examples
*
* Some recorded .oni files are available at <http://people.cs.pitt.edu/~chang/1635/proj11/kinectRecord>
*
* ``` shell
* LD_LIBRARY_PATH=/usr/lib/OpenNI2/Drivers/ gst-launch-1.0 --gst-debug=openni2src:5 openni2src location='Downloads/mr.oni' sourcetype=depth ! videoconvert ! ximagesink
* ```
*
* ``` shell
* LD_LIBRARY_PATH=/usr/lib/OpenNI2/Drivers/ gst-launch-1.0 --gst-debug=openni2src:5 openni2src location='Downloads/mr.oni' sourcetype=color ! videoconvert ! ximagesink
* ```
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "gstopenni2src.h"
GST_DEBUG_CATEGORY_STATIC (openni2src_debug);
#define GST_CAT_DEFAULT openni2src_debug
static GstStaticPadTemplate srctemplate = GST_STATIC_PAD_TEMPLATE ("src",
GST_PAD_SRC,
GST_PAD_ALWAYS,
GST_STATIC_CAPS (GST_VIDEO_CAPS_MAKE ("{RGBA, RGB, GRAY16_LE}"))
);
enum
{
PROP_0,
PROP_LOCATION,
PROP_SOURCETYPE
};
typedef enum
{
SOURCETYPE_DEPTH,
SOURCETYPE_COLOR,
SOURCETYPE_BOTH
} GstOpenni2SourceType;
#define DEFAULT_SOURCETYPE SOURCETYPE_DEPTH
#define SAMPLE_READ_WAIT_TIMEOUT 2000 /* 2000ms */
#define GST_TYPE_OPENNI2_SRC_SOURCETYPE (gst_openni2_src_sourcetype_get_type ())
static GType
gst_openni2_src_sourcetype_get_type (void)
{
static GType etype = 0;
if (etype == 0) {
static const GEnumValue values[] = {
{SOURCETYPE_DEPTH, "Get depth readings", "depth"},
{SOURCETYPE_COLOR, "Get color readings", "color"},
{SOURCETYPE_BOTH,
"Get color and depth (as alpha) readings - EXPERIMENTAL",
"both"},
{0, NULL, NULL},
};
etype = g_enum_register_static ("GstOpenni2SrcSourcetype", values);
}
return etype;
}
/* GObject methods */
static void gst_openni2_src_dispose (GObject * object);
static void gst_openni2_src_finalize (GObject * gobject);
static void gst_openni2_src_set_property (GObject * object, guint prop_id,
const GValue * value, GParamSpec * pspec);
static void gst_openni2_src_get_property (GObject * object, guint prop_id,
GValue * value, GParamSpec * pspec);
/* basesrc methods */
static gboolean gst_openni2_src_start (GstBaseSrc * bsrc);
static gboolean gst_openni2_src_stop (GstBaseSrc * bsrc);
static gboolean gst_openni2_src_set_caps (GstBaseSrc * src, GstCaps * caps);
static GstCaps *gst_openni2_src_get_caps (GstBaseSrc * src, GstCaps * filter);
static gboolean gst_openni2src_decide_allocation (GstBaseSrc * bsrc,
GstQuery * query);
/* element methods */
static GstStateChangeReturn gst_openni2_src_change_state (GstElement * element,
GstStateChange transition);
/* pushsrc method */
static GstFlowReturn gst_openni2src_fill (GstPushSrc * src, GstBuffer * buf);
/* OpenNI2 interaction methods */
static gboolean openni2_initialise_library ();
static gboolean openni2_initialise_devices (GstOpenni2Src * src);
static GstFlowReturn openni2_read_gstbuffer (GstOpenni2Src * src,
GstBuffer * buf);
#define parent_class gst_openni2_src_parent_class
G_DEFINE_TYPE (GstOpenni2Src, gst_openni2_src, GST_TYPE_PUSH_SRC);
static void
gst_openni2_src_class_init (GstOpenni2SrcClass * klass)
{
GObjectClass *gobject_class;
GstPushSrcClass *pushsrc_class;
GstBaseSrcClass *basesrc_class;
GstElementClass *element_class = GST_ELEMENT_CLASS (klass);
gobject_class = (GObjectClass *) klass;
basesrc_class = (GstBaseSrcClass *) klass;
pushsrc_class = (GstPushSrcClass *) klass;
gobject_class->dispose = gst_openni2_src_dispose;
gobject_class->finalize = gst_openni2_src_finalize;
gobject_class->set_property = gst_openni2_src_set_property;
gobject_class->get_property = gst_openni2_src_get_property;
g_object_class_install_property
(gobject_class, PROP_LOCATION,
g_param_spec_string ("location", "Location",
"Source uri, can be a file or a device.", "", (GParamFlags)
(G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
g_object_class_install_property (gobject_class, PROP_SOURCETYPE,
g_param_spec_enum ("sourcetype",
"Device source type",
"Type of readings to get from the source",
GST_TYPE_OPENNI2_SRC_SOURCETYPE, DEFAULT_SOURCETYPE,
(GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
basesrc_class->start = GST_DEBUG_FUNCPTR (gst_openni2_src_start);
basesrc_class->stop = GST_DEBUG_FUNCPTR (gst_openni2_src_stop);
basesrc_class->get_caps = GST_DEBUG_FUNCPTR (gst_openni2_src_get_caps);
basesrc_class->set_caps = GST_DEBUG_FUNCPTR (gst_openni2_src_set_caps);
basesrc_class->decide_allocation =
GST_DEBUG_FUNCPTR (gst_openni2src_decide_allocation);
gst_element_class_add_static_pad_template (element_class, &srctemplate);
gst_element_class_set_static_metadata (element_class, "Openni2 client source",
"Source/Video",
"Extract readings from an OpenNI supported device (Kinect etc). ",
"Miguel Casas-Sanchez <miguelecasassanchez@gmail.com>");
element_class->change_state = gst_openni2_src_change_state;
pushsrc_class->fill = GST_DEBUG_FUNCPTR (gst_openni2src_fill);
GST_DEBUG_CATEGORY_INIT (openni2src_debug, "openni2src", 0,
"OpenNI2 Device Source");
/* OpenNI2 initialisation inside this function */
openni2_initialise_library ();
}
static void
gst_openni2_src_init (GstOpenni2Src * ni2src)
{
gst_base_src_set_live (GST_BASE_SRC (ni2src), TRUE);
gst_base_src_set_format (GST_BASE_SRC (ni2src), GST_FORMAT_TIME);
ni2src->device = new openni::Device ();
ni2src->depth = new openni::VideoStream ();
ni2src->color = new openni::VideoStream ();
ni2src->depthFrame = new openni::VideoFrameRef ();
ni2src->colorFrame = new openni::VideoFrameRef ();
ni2src->oni_start_ts = GST_CLOCK_TIME_NONE;
}
static void
gst_openni2_src_dispose (GObject * object)
{
GstOpenni2Src *ni2src = GST_OPENNI2_SRC (object);
if (ni2src->gst_caps)
gst_caps_unref (ni2src->gst_caps);
G_OBJECT_CLASS (parent_class)->dispose (object);
}
static void
gst_openni2_src_finalize (GObject * gobject)
{
GstOpenni2Src *ni2src = GST_OPENNI2_SRC (gobject);
if (ni2src->uri_name) {
g_free (ni2src->uri_name);
ni2src->uri_name = NULL;
}
if (ni2src->gst_caps) {
gst_caps_unref (ni2src->gst_caps);
ni2src->gst_caps = NULL;
}
if (ni2src->device) {
delete ni2src->device;
ni2src->device = NULL;
}
if (ni2src->depth) {
delete ni2src->depth;
ni2src->depth = NULL;
}
if (ni2src->color) {
delete ni2src->color;
ni2src->color = NULL;
}
if (ni2src->depthFrame) {
delete ni2src->depthFrame;
ni2src->depthFrame = NULL;
}
if (ni2src->colorFrame) {
delete ni2src->colorFrame;
ni2src->colorFrame = NULL;
}
G_OBJECT_CLASS (parent_class)->finalize (gobject);
}
static void
gst_openni2_src_set_property (GObject * object, guint prop_id,
const GValue * value, GParamSpec * pspec)
{
GstOpenni2Src *openni2src = GST_OPENNI2_SRC (object);
GST_OBJECT_LOCK (openni2src);
switch (prop_id) {
case PROP_LOCATION:
if (!g_value_get_string (value)) {
GST_WARNING ("location property cannot be NULL");
break;
}
if (openni2src->uri_name != NULL) {
g_free (openni2src->uri_name);
openni2src->uri_name = NULL;
}
openni2src->uri_name = g_value_dup_string (value);
break;
case PROP_SOURCETYPE:
openni2src->sourcetype = g_value_get_enum (value);
break;
default:
G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
break;
}
GST_OBJECT_UNLOCK (openni2src);
}
static void
gst_openni2_src_get_property (GObject * object, guint prop_id,
GValue * value, GParamSpec * pspec)
{
GstOpenni2Src *openni2src = GST_OPENNI2_SRC (object);
GST_OBJECT_LOCK (openni2src);
switch (prop_id) {
case PROP_LOCATION:
g_value_set_string (value, openni2src->uri_name);
break;
case PROP_SOURCETYPE:
g_value_set_enum (value, openni2src->sourcetype);
break;
default:
G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
break;
}
GST_OBJECT_UNLOCK (openni2src);
}
/* Interesting info from gstv4l2src.c:
* "start and stop are not symmetric -- start will open the device, but not
* start capture. it's setcaps that will start capture, which is called via
* basesrc's negotiate method. stop will both stop capture and close t device."
*/
static gboolean
gst_openni2_src_start (GstBaseSrc * bsrc)
{
GstOpenni2Src *src = GST_OPENNI2_SRC (bsrc);
openni::Status rc = openni::STATUS_OK;
if (src->depth->isValid ()) {
rc = src->depth->start ();
if (rc != openni::STATUS_OK) {
GST_ERROR_OBJECT (src, "Couldn't start the depth stream: %s",
openni::OpenNI::getExtendedError ());
return FALSE;
}
}
if (src->color->isValid ()) {
rc = src->color->start ();
if (rc != openni::STATUS_OK) {
GST_ERROR_OBJECT (src, "Couldn't start the color stream: %s",
openni::OpenNI::getExtendedError ());
return FALSE;
}
}
return TRUE;
}
static gboolean
gst_openni2_src_stop (GstBaseSrc * bsrc)
{
GstOpenni2Src *src = GST_OPENNI2_SRC (bsrc);
if (src->depthFrame)
src->depthFrame->release ();
if (src->colorFrame)
src->colorFrame->release ();
if (src->depth->isValid ()) {
src->depth->stop ();
src->depth->destroy ();
}
if (src->color->isValid ()) {
src->color->stop ();
src->color->destroy ();
}
src->device->close ();
return TRUE;
}
static GstCaps *
gst_openni2_src_get_caps (GstBaseSrc * src, GstCaps * filter)
{
GstOpenni2Src *ni2src;
GstCaps *caps;
GstVideoInfo info;
GstVideoFormat format;
ni2src = GST_OPENNI2_SRC (src);
GST_OBJECT_LOCK (ni2src);
if (ni2src->gst_caps)
goto out;
// If we are here, we need to compose the caps and return them.
if (ni2src->depth->isValid () && ni2src->color->isValid () &&
ni2src->sourcetype == SOURCETYPE_BOTH
&& ni2src->colorpixfmt == openni::PIXEL_FORMAT_RGB888) {
format = GST_VIDEO_FORMAT_RGBA;
} else if (ni2src->depth->isValid () &&
ni2src->sourcetype == SOURCETYPE_DEPTH) {
format = GST_VIDEO_FORMAT_GRAY16_LE;
} else if (ni2src->color->isValid () && ni2src->sourcetype == SOURCETYPE_COLOR
&& ni2src->colorpixfmt == openni::PIXEL_FORMAT_RGB888) {
format = GST_VIDEO_FORMAT_RGB;
} else {
goto out;
}
gst_video_info_init (&info);
gst_video_info_set_format (&info, format, ni2src->width, ni2src->height);
info.fps_n = ni2src->fps;
info.fps_d = 1;
caps = gst_video_info_to_caps (&info);
GST_INFO_OBJECT (ni2src, "probed caps: %" GST_PTR_FORMAT, caps);
ni2src->gst_caps = caps;
out:
GST_OBJECT_UNLOCK (ni2src);
if (!ni2src->gst_caps)
return gst_pad_get_pad_template_caps (GST_BASE_SRC_PAD (ni2src));
return (filter)
? gst_caps_intersect_full (filter, ni2src->gst_caps,
GST_CAPS_INTERSECT_FIRST)
: gst_caps_ref (ni2src->gst_caps);
}
static gboolean
gst_openni2_src_set_caps (GstBaseSrc * src, GstCaps * caps)
{
GstOpenni2Src *ni2src;
ni2src = GST_OPENNI2_SRC (src);
return gst_video_info_from_caps (&ni2src->info, caps);
}
static GstStateChangeReturn
gst_openni2_src_change_state (GstElement * element, GstStateChange transition)
{
GstStateChangeReturn ret = GST_STATE_CHANGE_FAILURE;
GstOpenni2Src *src = GST_OPENNI2_SRC (element);
switch (transition) {
case GST_STATE_CHANGE_NULL_TO_READY:
/* Action! */
if (!openni2_initialise_devices (src))
return GST_STATE_CHANGE_FAILURE;
break;
case GST_STATE_CHANGE_READY_TO_PAUSED:
break;
case GST_STATE_CHANGE_PAUSED_TO_PLAYING:
break;
default:
break;
}
ret = GST_ELEMENT_CLASS (parent_class)->change_state (element, transition);
if (ret == GST_STATE_CHANGE_FAILURE) {
return ret;
}
switch (transition) {
case GST_STATE_CHANGE_READY_TO_NULL:
gst_openni2_src_stop (GST_BASE_SRC (src));
if (src->gst_caps) {
gst_caps_unref (src->gst_caps);
src->gst_caps = NULL;
}
break;
case GST_STATE_CHANGE_PLAYING_TO_PAUSED:
break;
case GST_STATE_CHANGE_PAUSED_TO_READY:
src->oni_start_ts = GST_CLOCK_TIME_NONE;
break;
default:
break;
}
return ret;
}
static GstFlowReturn
gst_openni2src_fill (GstPushSrc * src, GstBuffer * buf)
{
GstOpenni2Src *ni2src = GST_OPENNI2_SRC (src);
return openni2_read_gstbuffer (ni2src, buf);
}
static gboolean
gst_openni2src_decide_allocation (GstBaseSrc * bsrc, GstQuery * query)
{
GstBufferPool *pool;
guint size, min, max;
gboolean update;
GstStructure *config;
GstCaps *caps;
GstVideoInfo info;
gst_query_parse_allocation (query, &caps, NULL);
gst_video_info_from_caps (&info, caps);
if (gst_query_get_n_allocation_pools (query) > 0) {
gst_query_parse_nth_allocation_pool (query, 0, &pool, &size, &min, &max);
update = TRUE;
} else {
pool = NULL;
min = max = 0;
size = info.size;
update = FALSE;
}
GST_DEBUG_OBJECT (bsrc, "allocation: size:%u min:%u max:%u pool:%"
GST_PTR_FORMAT " caps:%" GST_PTR_FORMAT, size, min, max, pool, caps);
if (!pool)
pool = gst_video_buffer_pool_new ();
config = gst_buffer_pool_get_config (pool);
gst_buffer_pool_config_set_params (config, caps, size, min, max);
if (gst_query_find_allocation_meta (query, GST_VIDEO_META_API_TYPE, NULL)) {
GST_DEBUG_OBJECT (pool, "activate Video Meta");
gst_buffer_pool_config_add_option (config,
GST_BUFFER_POOL_OPTION_VIDEO_META);
}
gst_buffer_pool_set_config (pool, config);
if (update)
gst_query_set_nth_allocation_pool (query, 0, pool, size, min, max);
else
gst_query_add_allocation_pool (query, pool, size, min, max);
gst_object_unref (pool);
return GST_BASE_SRC_CLASS (parent_class)->decide_allocation (bsrc, query);
}
gboolean
gst_openni2src_plugin_init (GstPlugin * plugin)
{
return gst_element_register (plugin, "openni2src", GST_RANK_NONE,
GST_TYPE_OPENNI2_SRC);
}
static gboolean
openni2_initialise_library (void)
{
openni::Status rc = openni::STATUS_OK;
rc = openni::OpenNI::initialize ();
if (rc != openni::STATUS_OK) {
GST_ERROR ("Initialization failed: %s",
openni::OpenNI::getExtendedError ());
openni::OpenNI::shutdown ();
return GST_FLOW_ERROR;
}
return (rc == openni::STATUS_OK);
}
static gboolean
openni2_initialise_devices (GstOpenni2Src * src)
{
openni::Status rc = openni::STATUS_OK;
const char *deviceURI = openni::ANY_DEVICE;
if (src->uri_name)
deviceURI = src->uri_name;
rc = src->device->open (deviceURI);
if (rc != openni::STATUS_OK) {
GST_ERROR_OBJECT (src, "Device (%s) open failed: %s", deviceURI,
openni::OpenNI::getExtendedError ());
openni::OpenNI::shutdown ();
return FALSE;
}
/* depth sensor */
rc = src->depth->create (*src->device, openni::SENSOR_DEPTH);
if (rc == openni::STATUS_OK) {
rc = src->depth->start ();
if (rc != openni::STATUS_OK) {
GST_ERROR_OBJECT (src, "%s", openni::OpenNI::getExtendedError ());
src->depth->destroy ();
}
} else {
GST_WARNING_OBJECT (src, "Couldn't find depth stream: %s",
openni::OpenNI::getExtendedError ());
}
/* color sensor */
rc = src->color->create (*src->device, openni::SENSOR_COLOR);
if (rc == openni::STATUS_OK) {
rc = src->color->start ();
if (rc != openni::STATUS_OK) {
GST_ERROR_OBJECT (src, "Couldn't start color stream: %s ",
openni::OpenNI::getExtendedError ());
src->color->destroy ();
}
} else {
GST_WARNING_OBJECT (src, "Couldn't find color stream: %s",
openni::OpenNI::getExtendedError ());
}
if (!src->depth->isValid () && !src->color->isValid ()) {
GST_ERROR_OBJECT (src, "No valid streams. Exiting");
openni::OpenNI::shutdown ();
return FALSE;
}
/* Get resolution and make sure is valid */
if (src->depth->isValid () && src->color->isValid ()) {
src->depthVideoMode = src->depth->getVideoMode ();
src->colorVideoMode = src->color->getVideoMode ();
int depthWidth = src->depthVideoMode.getResolutionX ();
int depthHeight = src->depthVideoMode.getResolutionY ();
int colorWidth = src->colorVideoMode.getResolutionX ();
int colorHeight = src->colorVideoMode.getResolutionY ();
if (depthWidth == colorWidth && depthHeight == colorHeight) {
src->width = depthWidth;
src->height = depthHeight;
src->fps = src->depthVideoMode.getFps ();
src->colorpixfmt = src->colorVideoMode.getPixelFormat ();
src->depthpixfmt = src->depthVideoMode.getPixelFormat ();
} else {
GST_ERROR_OBJECT (src, "Error - expect color and depth to be"
" in same resolution: D: %dx%d vs C: %dx%d",
depthWidth, depthHeight, colorWidth, colorHeight);
return FALSE;
}
GST_INFO_OBJECT (src, "DEPTH&COLOR resolution: %dx%d",
src->width, src->height);
} else if (src->depth->isValid ()) {
src->depthVideoMode = src->depth->getVideoMode ();
src->width = src->depthVideoMode.getResolutionX ();
src->height = src->depthVideoMode.getResolutionY ();
src->fps = src->depthVideoMode.getFps ();
src->depthpixfmt = src->depthVideoMode.getPixelFormat ();
GST_INFO_OBJECT (src, "DEPTH resolution: %dx%d", src->width, src->height);
} else if (src->color->isValid ()) {
src->colorVideoMode = src->color->getVideoMode ();
src->width = src->colorVideoMode.getResolutionX ();
src->height = src->colorVideoMode.getResolutionY ();
src->fps = src->colorVideoMode.getFps ();
src->colorpixfmt = src->colorVideoMode.getPixelFormat ();
GST_INFO_OBJECT (src, "COLOR resolution: %dx%d", src->width, src->height);
} else {
GST_ERROR_OBJECT (src, "Expected at least one of the streams to be valid.");
return FALSE;
}
return TRUE;
}
static GstFlowReturn
openni2_read_gstbuffer (GstOpenni2Src * src, GstBuffer * buf)
{
openni::Status rc = openni::STATUS_OK;
openni::VideoStream * pStream = src->depth;
int changedStreamDummy;
GstVideoFrame vframe;
uint64_t oni_ts;
/* Block until we get some data */
rc = openni::OpenNI::waitForAnyStream (&pStream, 1, &changedStreamDummy,
SAMPLE_READ_WAIT_TIMEOUT);
if (rc != openni::STATUS_OK) {
GST_ERROR_OBJECT (src, "Frame read timeout: %s",
openni::OpenNI::getExtendedError ());
return GST_FLOW_ERROR;
}
if (src->depth->isValid () && src->color->isValid () &&
src->sourcetype == SOURCETYPE_BOTH) {
rc = src->depth->readFrame (src->depthFrame);
if (rc != openni::STATUS_OK) {
GST_ERROR_OBJECT (src, "Frame read error: %s",
openni::OpenNI::getExtendedError ());
return GST_FLOW_ERROR;
}
rc = src->color->readFrame (src->colorFrame);
if (rc != openni::STATUS_OK) {
GST_ERROR_OBJECT (src, "Frame read error: %s",
openni::OpenNI::getExtendedError ());
return GST_FLOW_ERROR;
}
/* Copy colour information */
gst_video_frame_map (&vframe, &src->info, buf, GST_MAP_WRITE);
guint8 *pData = (guint8 *) GST_VIDEO_FRAME_PLANE_DATA (&vframe, 0);
guint8 *pColor = (guint8 *) src->colorFrame->getData ();
/* Add depth as 8bit alpha channel, depth is 16bit samples. */
guint16 *pDepth = (guint16 *) src->depthFrame->getData ();
for (int i = 0; i < src->colorFrame->getHeight (); ++i) {
for (int j = 0; j < src->colorFrame->getWidth (); ++j) {
pData[4 * j + 0] = pColor[3 * j + 0];
pData[4 * j + 1] = pColor[3 * j + 1];
pData[4 * j + 2] = pColor[3 * j + 2];
pData[4 * j + 3] = pDepth[j] >> 8;
}
pData += GST_VIDEO_FRAME_PLANE_STRIDE (&vframe, 0);
pColor += src->colorFrame->getStrideInBytes ();
pDepth += src->depthFrame->getStrideInBytes () / 2;
}
gst_video_frame_unmap (&vframe);
oni_ts = src->colorFrame->getTimestamp () * 1000;
GST_LOG_OBJECT (src, "sending buffer (%d+%d)B",
src->colorFrame->getDataSize (),
src->depthFrame->getDataSize ());
} else if (src->depth->isValid () && src->sourcetype == SOURCETYPE_DEPTH) {
rc = src->depth->readFrame (src->depthFrame);
if (rc != openni::STATUS_OK) {
GST_ERROR_OBJECT (src, "Frame read error: %s",
openni::OpenNI::getExtendedError ());
return GST_FLOW_ERROR;
}
/* Copy depth information */
gst_video_frame_map (&vframe, &src->info, buf, GST_MAP_WRITE);
guint16 *pData = (guint16 *) GST_VIDEO_FRAME_PLANE_DATA (&vframe, 0);
guint16 *pDepth = (guint16 *) src->depthFrame->getData ();
for (int i = 0; i < src->depthFrame->getHeight (); ++i) {
memcpy (pData, pDepth, 2 * src->depthFrame->getWidth ());
pDepth += src->depthFrame->getStrideInBytes () / 2;
pData += GST_VIDEO_FRAME_PLANE_STRIDE (&vframe, 0) / 2;
}
gst_video_frame_unmap (&vframe);
oni_ts = src->depthFrame->getTimestamp () * 1000;
GST_LOG_OBJECT (src, "sending buffer (%dx%d)=%dB",
src->depthFrame->getWidth (),
src->depthFrame->getHeight (),
src->depthFrame->getDataSize ());
} else if (src->color->isValid () && src->sourcetype == SOURCETYPE_COLOR) {
rc = src->color->readFrame (src->colorFrame);
if (rc != openni::STATUS_OK) {
GST_ERROR_OBJECT (src, "Frame read error: %s",
openni::OpenNI::getExtendedError ());
return GST_FLOW_ERROR;
}
gst_video_frame_map (&vframe, &src->info, buf, GST_MAP_WRITE);
guint8 *pData = (guint8 *) GST_VIDEO_FRAME_PLANE_DATA (&vframe, 0);
guint8 *pColor = (guint8 *) src->colorFrame->getData ();
for (int i = 0; i < src->colorFrame->getHeight (); ++i) {
memcpy (pData, pColor, 3 * src->colorFrame->getWidth ());
pColor += src->colorFrame->getStrideInBytes ();
pData += GST_VIDEO_FRAME_PLANE_STRIDE (&vframe, 0);
}
gst_video_frame_unmap (&vframe);
oni_ts = src->colorFrame->getTimestamp () * 1000;
GST_LOG_OBJECT (src, "sending buffer (%dx%d)=%dB",
src->colorFrame->getWidth (),
src->colorFrame->getHeight (),
src->colorFrame->getDataSize ());
} else {
g_return_val_if_reached (GST_FLOW_ERROR);
return GST_FLOW_ERROR;
}
if (G_UNLIKELY (src->oni_start_ts == GST_CLOCK_TIME_NONE))
src->oni_start_ts = oni_ts;
GST_BUFFER_PTS (buf) = oni_ts - src->oni_start_ts;
GST_LOG_OBJECT (src, "Calculated PTS as %" GST_TIME_FORMAT,
GST_TIME_ARGS (GST_BUFFER_PTS (buf)));
return GST_FLOW_OK;
}