openni_device_primesense.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011 2011 Willow Garage, Inc.
00005  *    Suat Gedikli <gedikli@willowgarage.com>
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  */
00037 #include <openni_camera/openni_device_primesense.h>
00038 #include <openni_camera/openni_image_yuv_422.h>
00039 #include <iostream>
00040 #include <sstream>
00041 #include <boost/thread/mutex.hpp>
00042 
00043 using namespace boost;
00044 
00045 namespace openni_wrapper
00046 {
00047 
00048 DevicePrimesense::DevicePrimesense (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node) throw (OpenNIException)
00049 : OpenNIDevice (context, device_node, image_node, depth_node, ir_node)
00050 {
00051   // setup stream modes
00052   enumAvailableModes ();
00053   setDepthOutputMode (getDefaultDepthMode ());
00054   setImageOutputMode (getDefaultImageMode ());
00055   setIROutputMode (getDefaultIRMode ());
00056 
00057   unique_lock<mutex> image_lock(image_mutex_);
00058   XnStatus status = image_generator_.SetIntProperty ("InputFormat", 5);
00059   if (status != XN_STATUS_OK)
00060     THROW_OPENNI_EXCEPTION ("Error setting the image input format to Uncompressed 8-bit BAYER. Reason: %s", xnGetStatusString (status));
00061 
00062   status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_YUV422);
00063   if (status != XN_STATUS_OK)
00064     THROW_OPENNI_EXCEPTION ("Failed to set image pixel format to YUV422. Reason: %s", xnGetStatusString (status));
00065 
00066   image_lock.unlock ();
00067 
00068   lock_guard<mutex> depth_lock(depth_mutex_);
00069   status = depth_generator_.SetIntProperty ("RegistrationType", 1);
00070   if (status != XN_STATUS_OK)
00071     THROW_OPENNI_EXCEPTION ("Error setting the registration type. Reason: %s", xnGetStatusString (status));
00072 }
00073 
00074 DevicePrimesense::~DevicePrimesense () throw ()
00075 {
00076   setDepthRegistration ( false );
00077   setSynchronization ( false );
00078 
00079   depth_mutex_.lock ();
00080   depth_generator_.UnregisterFromNewDataAvailable (depth_callback_handle_);
00081   depth_mutex_.unlock ();
00082 
00083   image_mutex_.lock ();
00084   image_generator_.UnregisterFromNewDataAvailable (image_callback_handle_);
00085   image_mutex_.unlock ();
00086 }
00087 
00088 bool DevicePrimesense::isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw ()
00089 {
00090   return ImageYUV422::resizingSupported (input_width, input_height, output_width, output_height);
00091 }
00092 
00093 //void DevicePrimesense::setImageOutputMode (const XnMapOutputMode& output_mode) throw (OpenNIException)
00094 //{
00095 //  if (output_mode.nFPS == 30 && output_mode.nXRes == XN_UXGA_X_RES && output_mode.nYRes == XN_UXGA_Y_RES )
00096 //  {
00097 //    cout << "setting image mode to UXGA" << endl;
00098 //    if (isImageStreamRunning ())
00099 //    {
00100 //      stopImageStream ();
00101 //      XnStatus status = image_generator_.SetIntProperty ("InputFormat", 0);
00102 //      if (status != XN_STATUS_OK)
00103 //        THROW_OPENNI_EXCEPTION ("Error setting the image input format to compressed BAYER. Reason: %s", xnGetStatusString (status));
00104 //      status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_RGB24);
00105 //      if (status != XN_STATUS_OK)
00106 //        THROW_OPENNI_EXCEPTION ("Failed to set image pixel format to YUV422. Reason: %s", xnGetStatusString (status));
00107 //      sleep (1);
00108 //      startImageStream ();
00109 //    }
00110 //    else
00111 //    {
00112 //      XnStatus status = image_generator_.SetIntProperty ("InputFormat", 0);
00113 //      if (status != XN_STATUS_OK)
00114 //        THROW_OPENNI_EXCEPTION ("Error setting the image input format to compressed BAYER. Reason: %s", xnGetStatusString (status));
00115 //
00116 //      status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_GRAYSCALE_8_BIT);
00117 //      if (status != XN_STATUS_OK)
00118 //        THROW_OPENNI_EXCEPTION ("Failed to set image pixel format to YUV422. Reason: %s", xnGetStatusString (status));
00119 //    }
00120 //  }
00121 //  else
00122 //  {
00123 //    XnStatus status = image_generator_.SetIntProperty ("InputFormat", 5);
00124 //    if (status != XN_STATUS_OK)
00125 //        THROW_OPENNI_EXCEPTION ("Error setting the image input format to uncompressed BAYER. Reason: %s", xnGetStatusString (status));
00126 //
00127 //    status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_YUV422);
00128 //    if (status != XN_STATUS_OK)
00129 //      THROW_OPENNI_EXCEPTION ("Failed to set image pixel format to YUV422. Reason: %s", xnGetStatusString (status));
00130 //  }
00131 //  OpenNIDevice::setImageOutputMode (output_mode);
00132 //}
00133 
00134 void DevicePrimesense::enumAvailableModes () throw (OpenNIException)
00135 {
00136   XnMapOutputMode output_mode;
00137   available_image_modes_.clear();
00138   available_depth_modes_.clear();
00139 
00140   // Depth Modes
00141   output_mode.nFPS = 30;
00142   output_mode.nXRes = XN_VGA_X_RES;
00143   output_mode.nYRes = XN_VGA_Y_RES;
00144   available_depth_modes_.push_back (output_mode);
00145 
00146   output_mode.nFPS = 25;
00147   output_mode.nXRes = XN_VGA_X_RES;
00148   output_mode.nYRes = XN_VGA_Y_RES;
00149   available_depth_modes_.push_back (output_mode);
00150 
00151   output_mode.nFPS = 25;
00152   output_mode.nXRes = XN_QVGA_X_RES;
00153   output_mode.nYRes = XN_QVGA_Y_RES;
00154   available_depth_modes_.push_back (output_mode);
00155 
00156   output_mode.nFPS = 30;
00157   output_mode.nXRes = XN_QVGA_X_RES;
00158   output_mode.nYRes = XN_QVGA_Y_RES;
00159   available_depth_modes_.push_back (output_mode);
00160 
00161   output_mode.nFPS = 60;
00162   output_mode.nXRes = XN_QVGA_X_RES;
00163   output_mode.nYRes = XN_QVGA_Y_RES;
00164   available_depth_modes_.push_back (output_mode);
00165 
00166   // RGB Modes
00167   output_mode.nFPS = 30;
00168   output_mode.nXRes = XN_VGA_X_RES;
00169   output_mode.nYRes = XN_VGA_Y_RES;
00170   available_image_modes_.push_back (output_mode);
00171 
00172   output_mode.nFPS = 25;
00173   output_mode.nXRes = XN_VGA_X_RES;
00174   output_mode.nYRes = XN_VGA_Y_RES;
00175   available_image_modes_.push_back (output_mode);
00176 
00177 //  output_mode.nFPS = 30;
00178 //  output_mode.nXRes = XN_UXGA_X_RES;
00179 //  output_mode.nYRes = XN_UXGA_Y_RES;
00180 //  available_image_modes_.push_back (output_mode);
00181 
00182   output_mode.nFPS = 25;
00183   output_mode.nXRes = XN_QVGA_X_RES;
00184   output_mode.nYRes = XN_QVGA_Y_RES;
00185   available_image_modes_.push_back (output_mode);
00186 
00187   output_mode.nFPS = 30;
00188   output_mode.nXRes = XN_QVGA_X_RES;
00189   output_mode.nYRes = XN_QVGA_Y_RES;
00190   available_image_modes_.push_back (output_mode);
00191 
00192   output_mode.nFPS = 60;
00193   output_mode.nXRes = XN_QVGA_X_RES;
00194   output_mode.nYRes = XN_QVGA_Y_RES;
00195   available_image_modes_.push_back (output_mode);
00196 }
00197 
00198 boost::shared_ptr<Image> DevicePrimesense::getCurrentImage (boost::shared_ptr<xn::ImageMetaData> image_data) const throw ()
00199 {
00200   return boost::shared_ptr<Image> ( new ImageYUV422 (image_data) );
00201 }
00202 
00203 void DevicePrimesense::startImageStream () throw (OpenNIException)
00204 {
00205   // Suat: Ugly workaround... but on some usb-ports its not possible to start the image stream after the depth stream.
00206   // turning on and off registration solves for some reason the problem!
00207 
00208   if (isDepthStreamRunning ())
00209   {
00210     if (isDepthRegistered ())
00211     {
00212      // Reset the view point
00213       setDepthRegistration (false);
00214 
00215       // Reset the view point
00216       setDepthRegistration (true);
00217 
00218      // Reset the view point
00219       setDepthRegistration (false);
00220 
00221       // Start the stream
00222       OpenNIDevice::startImageStream ();
00223 
00224       // Register the stream
00225       setDepthRegistration (true);
00226     }
00227     else
00228     {
00229       // Reset the view point
00230       setDepthRegistration (true);
00231       // Reset the view point
00232       setDepthRegistration (false);
00233 
00234       // Start the stream
00235       OpenNIDevice::startImageStream ();
00236     }
00237   }
00238   else
00239     // Start the stream
00240     OpenNIDevice::startImageStream ();
00241 }
00242 
00243 void DevicePrimesense::startDepthStream () throw (OpenNIException)
00244 {
00245   if (isDepthRegistered ())
00246   {
00247     // Reset the view point
00248     setDepthRegistration (false);
00249 
00250     // Start the stream
00251     OpenNIDevice::startDepthStream ();
00252 
00253     // Register the stream
00254     setDepthRegistration (true);
00255   }
00256   else
00257     // Start the stream
00258     OpenNIDevice::startDepthStream ();
00259 }
00260 
00261 } //namespace


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Wed Aug 26 2015 15:08:49