Fawkes API  Fawkes Development Version
depth_thread.cpp
1 
2 /***************************************************************************
3  * depth_thread.cpp - OpenNI depth provider thread
4  *
5  * Created: Thu Dec 22 11:36:31 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "depth_thread.h"
24 
25 #include "utils/setup.h"
26 
27 #include <core/threading/mutex_locker.h>
28 #include <fvutils/color/colorspaces.h>
29 #include <fvutils/ipc/shm_image.h>
30 
31 #include <memory>
32 
33 using namespace fawkes;
34 using namespace firevision;
35 
36 /** @class OpenNiDepthThread "image_thread.h"
37  * OpenNI Depth Provider Thread.
38  * This thread provides RGB and depth images from the camera via a
39  * SharedMemoryImageBuffer to other FireVision plugins.
40  *
41  * @author Tim Niemueller
42  */
43 
44 /** Constructor. */
46 : Thread("OpenNiDepthThread", Thread::OPMODE_WAITFORWAKEUP),
47  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
48 {
49 }
50 
51 /** Destructor. */
53 {
54 }
55 
56 void
58 {
60 
61  depth_gen_ = new xn::DepthGenerator();
62 #if __cplusplus >= 201103L
63  std::unique_ptr<xn::DepthGenerator> depthgen_uniqueptr(depth_gen_);
64 #else
65  std::auto_ptr<xn::DepthGenerator> depthgen_uniqueptr(depth_gen_);
66 #endif
67 
68  XnStatus st;
69 
70  fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_DEPTH, depth_gen_);
71  fawkes::openni::setup_map_generator(*depth_gen_, config);
72 
73  depth_md_ = new xn::DepthMetaData();
74 
75  depth_gen_->GetMetaData(*depth_md_);
76 
77  depth_width_ = depth_md_->XRes();
78  depth_height_ = depth_md_->YRes();
79 
80  depth_buf_ =
81  new SharedMemoryImageBuffer("openni-depth", RAW16, depth_md_->XRes(), depth_md_->YRes());
82  depth_bufsize_ = colorspace_buffer_size(RAW16, depth_md_->XRes(), depth_md_->YRes());
83 
84  depth_gen_->StartGenerating();
85 
86  capture_start_ = new Time(clock);
87  capture_start_->stamp_systime();
88  // Update once to get timestamp
89  depth_gen_->WaitAndUpdateData();
90  // arbitrarily define the zero reference point,
91  // we can't get any closer than this
92  *capture_start_ -= (long int)depth_gen_->GetTimestamp();
93 
94  depthgen_uniqueptr.release();
95 }
96 
97 void
99 {
100  // we do not stop generating, we don't know if there is no other plugin
101  // using the node.
102  delete depth_gen_;
103  delete depth_md_;
104  delete depth_buf_;
105  delete capture_start_;
106 }
107 
108 void
110 {
112  bool is_depth_new = depth_gen_->IsDataNew();
113  depth_gen_->GetMetaData(*depth_md_);
114  const XnDepthPixel *const depth_data = depth_md_->Data();
115  fawkes::Time ts = *capture_start_ + (long int)depth_gen_->GetTimestamp();
116  lock.unlock();
117 
118  if (is_depth_new && (depth_buf_->num_attached() > 1)) {
119  memcpy(depth_buf_->buffer(), depth_data, depth_bufsize_);
120  }
121 
122  depth_buf_->set_capture_time(&ts);
123 }
firevision::SharedMemoryImageBuffer::set_capture_time
void set_capture_time(fawkes::Time *time)
Set the capture time.
Definition: shm_image.cpp:198
fawkes::SharedMemory::num_attached
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:763
fawkes::MutexLocker
Mutex locking helper.
Definition: mutex_locker.h:34
fawkes::LockPtr::objmutex_ptr
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:284
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
firevision::SharedMemoryImageBuffer
Shared memory image buffer.
Definition: shm_image.h:184
fawkes::Time::stamp_systime
Time & stamp_systime()
Set this time to the current system time.
Definition: time.cpp:720
OpenNiDepthThread::OpenNiDepthThread
OpenNiDepthThread()
Constructor.
Definition: depth_thread.cpp:45
OpenNiDepthThread::~OpenNiDepthThread
virtual ~OpenNiDepthThread()
Destructor.
Definition: depth_thread.cpp:52
fawkes::ClockAspect::clock
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
OpenNiDepthThread::finalize
virtual void finalize()
Finalize the thread.
Definition: depth_thread.cpp:98
OpenNiDepthThread::loop
virtual void loop()
Code to execute in the thread.
Definition: depth_thread.cpp:109
fawkes
Fawkes library namespace.
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
firevision::SharedMemoryImageBuffer::buffer
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:228
fawkes::Time
A class for handling time.
Definition: time.h:93
OpenNiDepthThread::init
virtual void init()
Initialize the thread.
Definition: depth_thread.cpp:57
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::MutexLocker::unlock
void unlock()
Unlock the mutex.
Definition: mutex_locker.cpp:153
fawkes::OpenNiAspect::openni
LockPtr< xn::Context > openni
Central OpenNI context.
Definition: openni.h:47