Fawkes API  Fawkes Development Version
depth_thread.cpp
00001 
00002 /***************************************************************************
00003  *  depth_thread.cpp - OpenNI depth provider thread
00004  *
00005  *  Created: Thu Dec 22 11:36:31 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "depth_thread.h"
00024 #include "utils/setup.h"
00025 
00026 #include <core/threading/mutex_locker.h>
00027 #include <fvutils/ipc/shm_image.h>
00028 #include <fvutils/color/colorspaces.h>
00029 
00030 #include <memory>
00031 
00032 using namespace fawkes;
00033 using namespace firevision;
00034 
00035 /** @class OpenNiDepthThread "image_thread.h"
00036  * OpenNI Depth Provider Thread.
00037  * This thread provides RGB and depth images from the camera via a
00038  * SharedMemoryImageBuffer to other FireVision plugins.
00039  *
00040  * @author Tim Niemueller
00041  */
00042 
00043 /** Constructor. */
00044 OpenNiDepthThread::OpenNiDepthThread()
00045   : Thread("OpenNiDepthThread", Thread::OPMODE_WAITFORWAKEUP),
00046     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
00047 {
00048 }
00049 
00050 
00051 /** Destructor. */
00052 OpenNiDepthThread::~OpenNiDepthThread()
00053 {
00054 }
00055 
00056 
00057 void
00058 OpenNiDepthThread::init()
00059 {
00060   MutexLocker lock(openni.objmutex_ptr());
00061 
00062   __depth_gen = new xn::DepthGenerator();
00063   std::auto_ptr<xn::DepthGenerator> depthgen_autoptr(__depth_gen);
00064 
00065   XnStatus st;
00066 
00067   fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_DEPTH, __depth_gen);
00068   fawkes::openni::setup_map_generator(*__depth_gen, config);
00069 
00070   __depth_md = new xn::DepthMetaData();
00071 
00072   __depth_gen->GetMetaData(*__depth_md);
00073 
00074   __depth_width  = __depth_md->XRes();
00075   __depth_height = __depth_md->YRes();
00076 
00077   __depth_buf = new SharedMemoryImageBuffer("openni-depth", RAW16,
00078                                             __depth_md->XRes(), __depth_md->YRes());
00079   __depth_bufsize = colorspace_buffer_size(RAW16,
00080                                            __depth_md->XRes(), __depth_md->YRes());
00081 
00082   __depth_gen->StartGenerating();
00083 
00084   __capture_start = new Time(clock);
00085   __capture_start->stamp_systime();
00086   // Update once to get timestamp
00087   __depth_gen->WaitAndUpdateData();
00088   // arbitrarily define the zero reference point,
00089   // we can't get any closer than this
00090   *__capture_start -= (long int)__depth_gen->GetTimestamp();
00091   
00092   
00093   depthgen_autoptr.release();
00094 }
00095 
00096 
00097 void
00098 OpenNiDepthThread::finalize()
00099 {
00100   // we do not stop generating, we don't know if there is no other plugin
00101   // using the node.
00102   delete __depth_gen;
00103   delete __depth_md;
00104   delete __depth_buf;
00105 }
00106 
00107 
00108 void
00109 OpenNiDepthThread::loop()
00110 {
00111   MutexLocker lock(openni.objmutex_ptr());
00112   bool is_depth_new = __depth_gen->IsDataNew();
00113   __depth_gen->GetMetaData(*__depth_md);
00114   const XnDepthPixel * const depth_data = __depth_md->Data();
00115   fawkes::Time ts = *__capture_start + (long int)__depth_gen->GetTimestamp();
00116   lock.unlock();
00117 
00118   if (is_depth_new && (__depth_buf->num_attached() > 1)) {
00119     memcpy(__depth_buf->buffer(), depth_data, __depth_bufsize);
00120   }
00121 
00122   __depth_buf->set_capture_time(&ts);
00123 }