Fawkes API
Fawkes Development Version
|
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 }