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 std;
00044 using namespace boost;
00045 
00046 namespace openni_wrapper
00047 {
00048 
00049 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)
00050 : OpenNIDevice (context, device_node, image_node, depth_node, ir_node)
00051 {
00052   // setup stream modes
00053   enumAvailableModes ();
00054   setDepthOutputMode (getDefaultDepthMode ());
00055   setImageOutputMode (getDefaultImageMode ());
00056   setIROutputMode (getDefaultIRMode ());
00057 
00058   unique_lock<mutex> image_lock(image_mutex_);
00059   XnStatus status = image_generator_.SetIntProperty ("InputFormat", 5);
00060   if (status != XN_STATUS_OK)
00061     THROW_OPENNI_EXCEPTION ("Error setting the image input format to Uncompressed 8-bit BAYER. Reason: %s", xnGetStatusString (status));
00062 
00063   status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_YUV422);
00064   if (status != XN_STATUS_OK)
00065     THROW_OPENNI_EXCEPTION ("Failed to set image pixel format to YUV422. Reason: %s", xnGetStatusString (status));
00066 
00067   image_lock.unlock ();
00068 
00069   lock_guard<mutex> depth_lock(depth_mutex_);
00070   status = depth_generator_.SetIntProperty ("RegistrationType", 1);
00071   if (status != XN_STATUS_OK)
00072     THROW_OPENNI_EXCEPTION ("Error setting the registration type. Reason: %s", xnGetStatusString (status));
00073 }
00074 
00075 DevicePrimesense::~DevicePrimesense () throw ()
00076 {
00077   setDepthRegistration ( false );
00078   setSynchronization ( false );
00079 
00080   depth_mutex_.lock ();
00081   depth_generator_.UnregisterFromNewDataAvailable (depth_callback_handle_);
00082   depth_mutex_.unlock ();
00083 
00084   image_mutex_.lock ();
00085   image_generator_.UnregisterFromNewDataAvailable (image_callback_handle_);
00086   image_mutex_.unlock ();
00087 }
00088 
00089 bool DevicePrimesense::isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw ()
00090 {
00091   return ImageYUV422::resizingSupported (input_width, input_height, output_width, output_height);
00092 }
00093 
00094 //void DevicePrimesense::setImageOutputMode (const XnMapOutputMode& output_mode) throw (OpenNIException)
00095 //{
00096 //  if (output_mode.nFPS == 30 && output_mode.nXRes == XN_UXGA_X_RES && output_mode.nYRes == XN_UXGA_Y_RES )
00097 //  {
00098 //    cout << "setting image mode to UXGA" << endl;
00099 //    if (isImageStreamRunning ())
00100 //    {
00101 //      stopImageStream ();
00102 //      XnStatus status = image_generator_.SetIntProperty ("InputFormat", 0);
00103 //      if (status != XN_STATUS_OK)
00104 //        THROW_OPENNI_EXCEPTION ("Error setting the image input format to compressed BAYER. Reason: %s", xnGetStatusString (status));
00105 //      status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_RGB24);
00106 //      if (status != XN_STATUS_OK)
00107 //        THROW_OPENNI_EXCEPTION ("Failed to set image pixel format to YUV422. Reason: %s", xnGetStatusString (status));
00108 //      sleep (1);
00109 //      startImageStream ();
00110 //    }
00111 //    else
00112 //    {
00113 //      XnStatus status = image_generator_.SetIntProperty ("InputFormat", 0);
00114 //      if (status != XN_STATUS_OK)
00115 //        THROW_OPENNI_EXCEPTION ("Error setting the image input format to compressed BAYER. Reason: %s", xnGetStatusString (status));
00116 //
00117 //      status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_GRAYSCALE_8_BIT);
00118 //      if (status != XN_STATUS_OK)
00119 //        THROW_OPENNI_EXCEPTION ("Failed to set image pixel format to YUV422. Reason: %s", xnGetStatusString (status));
00120 //    }
00121 //  }
00122 //  else
00123 //  {
00124 //    XnStatus status = image_generator_.SetIntProperty ("InputFormat", 5);
00125 //    if (status != XN_STATUS_OK)
00126 //        THROW_OPENNI_EXCEPTION ("Error setting the image input format to uncompressed BAYER. Reason: %s", xnGetStatusString (status));
00127 //
00128 //    status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_YUV422);
00129 //    if (status != XN_STATUS_OK)
00130 //      THROW_OPENNI_EXCEPTION ("Failed to set image pixel format to YUV422. Reason: %s", xnGetStatusString (status));
00131 //  }
00132 //  OpenNIDevice::setImageOutputMode (output_mode);
00133 //}
00134 
00135 void DevicePrimesense::enumAvailableModes () throw (OpenNIException)
00136 {
00137   XnMapOutputMode output_mode;
00138   available_image_modes_.clear();
00139   available_depth_modes_.clear();
00140 
00141   // Depth Modes
00142   output_mode.nFPS = 30;
00143   output_mode.nXRes = XN_VGA_X_RES;
00144   output_mode.nYRes = XN_VGA_Y_RES;
00145   available_depth_modes_.push_back (output_mode);
00146 
00147   output_mode.nFPS = 25;
00148   output_mode.nXRes = XN_VGA_X_RES;
00149   output_mode.nYRes = XN_VGA_Y_RES;
00150   available_depth_modes_.push_back (output_mode);
00151 
00152   output_mode.nFPS = 25;
00153   output_mode.nXRes = XN_QVGA_X_RES;
00154   output_mode.nYRes = XN_QVGA_Y_RES;
00155   available_depth_modes_.push_back (output_mode);
00156 
00157   output_mode.nFPS = 30;
00158   output_mode.nXRes = XN_QVGA_X_RES;
00159   output_mode.nYRes = XN_QVGA_Y_RES;
00160   available_depth_modes_.push_back (output_mode);
00161 
00162   output_mode.nFPS = 60;
00163   output_mode.nXRes = XN_QVGA_X_RES;
00164   output_mode.nYRes = XN_QVGA_Y_RES;
00165   available_depth_modes_.push_back (output_mode);
00166 
00167   // RGB Modes
00168   output_mode.nFPS = 30;
00169   output_mode.nXRes = XN_VGA_X_RES;
00170   output_mode.nYRes = XN_VGA_Y_RES;
00171   available_image_modes_.push_back (output_mode);
00172 
00173   output_mode.nFPS = 25;
00174   output_mode.nXRes = XN_VGA_X_RES;
00175   output_mode.nYRes = XN_VGA_Y_RES;
00176   available_image_modes_.push_back (output_mode);
00177 
00178 //  output_mode.nFPS = 30;
00179 //  output_mode.nXRes = XN_UXGA_X_RES;
00180 //  output_mode.nYRes = XN_UXGA_Y_RES;
00181 //  available_image_modes_.push_back (output_mode);
00182 
00183   output_mode.nFPS = 25;
00184   output_mode.nXRes = XN_QVGA_X_RES;
00185   output_mode.nYRes = XN_QVGA_Y_RES;
00186   available_image_modes_.push_back (output_mode);
00187 
00188   output_mode.nFPS = 30;
00189   output_mode.nXRes = XN_QVGA_X_RES;
00190   output_mode.nYRes = XN_QVGA_Y_RES;
00191   available_image_modes_.push_back (output_mode);
00192 
00193   output_mode.nFPS = 60;
00194   output_mode.nXRes = XN_QVGA_X_RES;
00195   output_mode.nYRes = XN_QVGA_Y_RES;
00196   available_image_modes_.push_back (output_mode);
00197 }
00198 
00199 boost::shared_ptr<Image> DevicePrimesense::getCurrentImage (boost::shared_ptr<xn::ImageMetaData> image_data) const throw ()
00200 {
00201   return boost::shared_ptr<Image> ( new ImageYUV422 (image_data) );
00202 }
00203 
00204 void DevicePrimesense::startImageStream () throw (OpenNIException)
00205 {
00206   // Suat: Ugly workaround... but on some usb-ports its not possible to start the image stream after the depth stream.
00207   // turning on and off registration solves for some reason the problem!
00208 
00209   if (isDepthStreamRunning ())
00210   {
00211     if (isDepthRegistered ())
00212     {
00213      // Reset the view point
00214       setDepthRegistration (false);
00215 
00216       // Reset the view point
00217       setDepthRegistration (true);
00218 
00219      // Reset the view point
00220       setDepthRegistration (false);
00221 
00222       // Start the stream
00223       OpenNIDevice::startImageStream ();
00224 
00225       // Register the stream
00226       setDepthRegistration (true);
00227     }
00228     else
00229     {
00230       // Reset the view point
00231       setDepthRegistration (true);
00232       // Reset the view point
00233       setDepthRegistration (false);
00234 
00235       // Start the stream
00236       OpenNIDevice::startImageStream ();
00237     }
00238   }
00239   else
00240     // Start the stream
00241     OpenNIDevice::startImageStream ();
00242 }
00243 
00244 void DevicePrimesense::startDepthStream () throw (OpenNIException)
00245 {
00246   if (isDepthRegistered ())
00247   {
00248     // Reset the view point
00249     setDepthRegistration (false);
00250 
00251     // Start the stream
00252     OpenNIDevice::startDepthStream ();
00253 
00254     // Register the stream
00255     setDepthRegistration (true);
00256   }
00257   else
00258     // Start the stream
00259     OpenNIDevice::startDepthStream ();
00260 }
00261 
00262 } //namespace
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Thu Aug 15 2013 10:49:03