mirror of
https://gitlab.freedesktop.org/gstreamer/gstreamer.git
synced 2024-09-09 05:38:58 +00:00
642 lines
21 KiB
C++
642 lines
21 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
|
|
*
|
|
* <refsect2>
|
|
* <title>Examples</title>
|
|
* <para>
|
|
* Some recorded .oni files are available at:
|
|
* <programlisting>
|
|
* http://people.cs.pitt.edu/~chang/1635/proj11/kinectRecord
|
|
* </programlisting>
|
|
*
|
|
* <programlisting>
|
|
LD_LIBRARY_PATH=/usr/lib/OpenNI2/Drivers/ gst-launch-1.0 --gst-debug=openni2src:5 openni2src location='Downloads/mr.oni' sourcetype=depth ! videoconvert ! ximagesink
|
|
* </programlisting>
|
|
* <programlisting>
|
|
LD_LIBRARY_PATH=/usr/lib/OpenNI2/Drivers/ gst-launch-1.0 --gst-debug=openni2src:5 openni2src location='Downloads/mr.oni' sourcetype=color ! videoconvert ! ximagesink
|
|
* </programlisting>
|
|
* </para>
|
|
* </refsect2>
|
|
*/
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
#include "config.h"
|
|
#endif
|
|
|
|
#include "gstopenni2src.h"
|
|
|
|
GST_DEBUG_CATEGORY_STATIC (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 GstCaps *gst_openni2_src_get_caps (GstBaseSrc * src, GstCaps * filter);
|
|
|
|
/* 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 GstFlowReturn openni2_initialise_devices (GstOpenni2Src * src);
|
|
static GstFlowReturn openni2_read_gstbuffer (GstOpenni2Src * src,
|
|
GstBuffer * buf);
|
|
static void openni2_finalise (GstOpenni2Src * src);
|
|
|
|
#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);
|
|
|
|
gst_element_class_add_pad_template (element_class,
|
|
gst_static_pad_template_get (&srctemplate));
|
|
|
|
gst_element_class_set_static_metadata (element_class, "Openni2 client source",
|
|
"Source/Device",
|
|
"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_format (GST_BASE_SRC (ni2src), GST_FORMAT_BYTES);
|
|
}
|
|
|
|
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);
|
|
|
|
openni2_finalise (ni2src);
|
|
|
|
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;
|
|
|
|
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);
|
|
|
|
/* Action! */
|
|
openni2_initialise_devices (openni2src);
|
|
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\n%s\n",
|
|
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\n%s\n",
|
|
openni::OpenNI::getExtendedError ());
|
|
return FALSE;
|
|
}
|
|
}
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
static gboolean
|
|
gst_openni2_src_stop (GstBaseSrc * bsrc)
|
|
{
|
|
GstOpenni2Src *src = GST_OPENNI2_SRC (bsrc);
|
|
|
|
if (src->depth.isValid ())
|
|
src->depth.stop ();
|
|
if (src->color.isValid ())
|
|
src->color.stop ();
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
static GstCaps *
|
|
gst_openni2_src_get_caps (GstBaseSrc * src, GstCaps * filter)
|
|
{
|
|
GstOpenni2Src *ni2src;
|
|
GstCaps *caps;
|
|
|
|
ni2src = GST_OPENNI2_SRC (src);
|
|
|
|
GST_OBJECT_LOCK (ni2src);
|
|
if (ni2src->gst_caps) {
|
|
GST_OBJECT_UNLOCK (ni2src);
|
|
return (filter)
|
|
? gst_caps_intersect_full (filter, ni2src->gst_caps,
|
|
GST_CAPS_INTERSECT_FIRST)
|
|
: gst_caps_ref (ni2src->gst_caps);
|
|
}
|
|
// If we are here, we need to compose the caps and return them.
|
|
caps = gst_caps_new_empty ();
|
|
if (ni2src->colorpixfmt != openni::PIXEL_FORMAT_RGB888)
|
|
return caps; /* Uh oh, not RGB :? Not supported. */
|
|
|
|
if (ni2src->depth.isValid () && ni2src->color.isValid () &&
|
|
ni2src->sourcetype == SOURCETYPE_BOTH) {
|
|
caps = gst_caps_new_simple ("video/x-raw",
|
|
"format", G_TYPE_STRING, "RGBA",
|
|
"framerate", GST_TYPE_FRACTION, ni2src->fps, 1,
|
|
"pixel-aspect-ratio", GST_TYPE_FRACTION, 1, 1,
|
|
"width", G_TYPE_INT, ni2src->width,
|
|
"height", G_TYPE_INT, ni2src->height, NULL);
|
|
} else if (ni2src->depth.isValid () && ni2src->sourcetype == SOURCETYPE_DEPTH) {
|
|
caps = gst_caps_new_simple ("video/x-raw",
|
|
"format", G_TYPE_STRING, "GRAY16_LE",
|
|
"framerate", GST_TYPE_FRACTION, ni2src->fps, 1,
|
|
"pixel-aspect-ratio", GST_TYPE_FRACTION, 1, 1,
|
|
"width", G_TYPE_INT, ni2src->width,
|
|
"height", G_TYPE_INT, ni2src->height, NULL);
|
|
} else if (ni2src->color.isValid () && ni2src->sourcetype == SOURCETYPE_COLOR) {
|
|
caps = gst_caps_new_simple ("video/x-raw",
|
|
"format", G_TYPE_STRING, "RGB",
|
|
"framerate", GST_TYPE_FRACTION, ni2src->fps, 1,
|
|
"pixel-aspect-ratio", GST_TYPE_FRACTION, 1, 1,
|
|
"width", G_TYPE_INT, ni2src->width,
|
|
"height", G_TYPE_INT, ni2src->height, NULL);
|
|
}
|
|
GST_INFO_OBJECT (ni2src, "probed caps: %" GST_PTR_FORMAT, caps);
|
|
ni2src->gst_caps = gst_caps_ref (caps);
|
|
GST_OBJECT_UNLOCK (ni2src);
|
|
return (filter)
|
|
? gst_caps_intersect_full (filter, ni2src->gst_caps,
|
|
GST_CAPS_INTERSECT_FIRST)
|
|
: gst_caps_ref (ni2src->gst_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:
|
|
break;
|
|
case GST_STATE_CHANGE_READY_TO_PAUSED:
|
|
if (!src->uri_name) {
|
|
GST_ERROR_OBJECT (src, "Invalid location");
|
|
return ret;
|
|
}
|
|
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:
|
|
case GST_STATE_CHANGE_PAUSED_TO_READY:
|
|
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);
|
|
}
|
|
|
|
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 ()
|
|
{
|
|
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);
|
|
}
|
|
|
|
GstFlowReturn
|
|
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;
|
|
|
|
/** OpenNI2 open device or file **/
|
|
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 GST_FLOW_ERROR;
|
|
}
|
|
|
|
/** 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\n");
|
|
openni::OpenNI::shutdown ();
|
|
return GST_FLOW_ERROR;
|
|
}
|
|
|
|
/** 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 GST_FLOW_ERROR;
|
|
}
|
|
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 GST_FLOW_ERROR;
|
|
}
|
|
|
|
return GST_FLOW_OK;
|
|
}
|
|
|
|
static GstFlowReturn
|
|
openni2_read_gstbuffer (GstOpenni2Src * src, GstBuffer * buf)
|
|
{
|
|
openni::Status rc = openni::STATUS_OK;
|
|
openni::VideoStream * pStream = &(src->depth);
|
|
int changedStreamDummy;
|
|
GstMapInfo info;
|
|
|
|
/* 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;
|
|
}
|
|
if ((src->colorFrame.getStrideInBytes () != src->colorFrame.getWidth ()) ||
|
|
(src->depthFrame.getStrideInBytes () !=
|
|
2 * src->depthFrame.getWidth ())) {
|
|
// This case is not handled - yet :B
|
|
GST_ERROR_OBJECT (src, "Stride does not coincide with width");
|
|
return GST_FLOW_ERROR;
|
|
}
|
|
|
|
int framesize =
|
|
src->colorFrame.getDataSize () + src->depthFrame.getDataSize () / 2;
|
|
buf = gst_buffer_new_and_alloc (framesize);
|
|
/* Copy colour information */
|
|
gst_buffer_map (buf, &info, (GstMapFlags) (GST_MAP_WRITE));
|
|
memcpy (info.data, src->colorFrame.getData (),
|
|
src->colorFrame.getDataSize ());
|
|
guint8 *pData = info.data + src->colorFrame.getDataSize ();
|
|
/* Add depth as 8bit alpha channel, depth is 16bit samples. */
|
|
guint16 *pDepth = (guint16 *) src->depthFrame.getData ();
|
|
for (int i = 0; i < src->depthFrame.getDataSize () / 2; ++i)
|
|
pData[i] = pDepth[i] >> 8;
|
|
GST_BUFFER_PTS (buf) = src->colorFrame.getTimestamp () * 1000;
|
|
GST_LOG_OBJECT (src, "sending buffer (%d+%d)B [%08llu]",
|
|
src->colorFrame.getDataSize (),
|
|
src->depthFrame.getDataSize (),
|
|
(long long) src->depthFrame.getTimestamp ());
|
|
gst_buffer_unmap (buf, &info);
|
|
} 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;
|
|
}
|
|
if (src->depthFrame.getStrideInBytes () != 2 * src->depthFrame.getWidth ()) {
|
|
// This case is not handled - yet :B
|
|
GST_ERROR_OBJECT (src, "Stride does not coincide with width");
|
|
return GST_FLOW_ERROR;
|
|
}
|
|
|
|
int framesize = src->depthFrame.getDataSize ();
|
|
buf = gst_buffer_new_and_alloc (framesize);
|
|
gst_buffer_map (buf, &info, (GstMapFlags) (GST_MAP_WRITE));
|
|
memcpy (info.data, src->depthFrame.getData (), framesize);
|
|
GST_BUFFER_PTS (buf) = src->depthFrame.getTimestamp () * 1000;
|
|
GST_LOG_OBJECT (src, "sending buffer (%dx%d)=%dB [%08llu]",
|
|
src->depthFrame.getWidth (),
|
|
src->depthFrame.getHeight (),
|
|
src->depthFrame.getDataSize (),
|
|
(long long) src->depthFrame.getTimestamp ());
|
|
gst_buffer_unmap (buf, &info);
|
|
} 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;
|
|
}
|
|
if (src->colorFrame.getStrideInBytes () != src->colorFrame.getWidth ()) {
|
|
// This case is not handled - yet :B
|
|
GST_ERROR_OBJECT (src, "Stride does not coincide with width");
|
|
return GST_FLOW_ERROR;
|
|
}
|
|
|
|
int framesize = src->colorFrame.getDataSize ();
|
|
buf = gst_buffer_new_and_alloc (framesize);
|
|
gst_buffer_map (buf, &info, (GstMapFlags) (GST_MAP_WRITE));
|
|
memcpy (info.data, src->depthFrame.getData (), framesize);
|
|
GST_BUFFER_PTS (buf) = src->colorFrame.getTimestamp () * 1000;
|
|
GST_LOG_OBJECT (src, "sending buffer (%dx%d)=%dB [%08llu]",
|
|
src->colorFrame.getWidth (),
|
|
src->colorFrame.getHeight (),
|
|
src->colorFrame.getDataSize (),
|
|
(long long) src->colorFrame.getTimestamp ());
|
|
gst_buffer_unmap (buf, &info);
|
|
}
|
|
return GST_FLOW_OK;
|
|
}
|
|
|
|
static void
|
|
openni2_finalise (GstOpenni2Src * src)
|
|
{
|
|
src->depth.destroy ();
|
|
src->color.destroy ();
|
|
openni::OpenNI::shutdown ();
|
|
}
|