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 #include "utils/setup.h"
25 
26 #include <core/threading/mutex_locker.h>
27 #include <fvutils/ipc/shm_image.h>
28 #include <fvutils/color/colorspaces.h>
29 
30 #include <memory>
31 
32 using namespace fawkes;
33 using namespace firevision;
34 
35 /** @class OpenNiDepthThread "image_thread.h"
36  * OpenNI Depth Provider Thread.
37  * This thread provides RGB and depth images from the camera via a
38  * SharedMemoryImageBuffer to other FireVision plugins.
39  *
40  * @author Tim Niemueller
41  */
42 
43 /** Constructor. */
45  : Thread("OpenNiDepthThread", Thread::OPMODE_WAITFORWAKEUP),
46  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
47 {
48 }
49 
50 
51 /** Destructor. */
53 {
54 }
55 
56 
57 void
59 {
61 
62  __depth_gen = new xn::DepthGenerator();
63 #if __cplusplus >= 201103L
64  std::unique_ptr<xn::DepthGenerator> depthgen_uniqueptr(__depth_gen);
65 #else
66  std::auto_ptr<xn::DepthGenerator> depthgen_uniqueptr(__depth_gen);
67 #endif
68 
69  XnStatus st;
70 
71  fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_DEPTH, __depth_gen);
72  fawkes::openni::setup_map_generator(*__depth_gen, config);
73 
74  __depth_md = new xn::DepthMetaData();
75 
76  __depth_gen->GetMetaData(*__depth_md);
77 
78  __depth_width = __depth_md->XRes();
79  __depth_height = __depth_md->YRes();
80 
81  __depth_buf = new SharedMemoryImageBuffer("openni-depth", RAW16,
82  __depth_md->XRes(), __depth_md->YRes());
83  __depth_bufsize = colorspace_buffer_size(RAW16,
84  __depth_md->XRes(), __depth_md->YRes());
85 
86  __depth_gen->StartGenerating();
87 
88  __capture_start = new Time(clock);
89  __capture_start->stamp_systime();
90  // Update once to get timestamp
91  __depth_gen->WaitAndUpdateData();
92  // arbitrarily define the zero reference point,
93  // we can't get any closer than this
94  *__capture_start -= (long int)__depth_gen->GetTimestamp();
95 
96 
97  depthgen_uniqueptr.release();
98 }
99 
100 
101 void
103 {
104  // we do not stop generating, we don't know if there is no other plugin
105  // using the node.
106  delete __depth_gen;
107  delete __depth_md;
108  delete __depth_buf;
109 }
110 
111 
112 void
114 {
116  bool is_depth_new = __depth_gen->IsDataNew();
117  __depth_gen->GetMetaData(*__depth_md);
118  const XnDepthPixel * const depth_data = __depth_md->Data();
119  fawkes::Time ts = *__capture_start + (long int)__depth_gen->GetTimestamp();
120  lock.unlock();
121 
122  if (is_depth_new && (__depth_buf->num_attached() > 1)) {
123  memcpy(__depth_buf->buffer(), depth_data, __depth_bufsize);
124  }
125 
126  __depth_buf->set_capture_time(&ts);
127 }
LockPtr< xn::Context > openni
Central OpenNI context.
Definition: openni.h:48
Time & stamp_systime()
Set this time to the current system time.
Definition: time.cpp:800
Fawkes library namespace.
Mutex locking helper.
Definition: mutex_locker.h:33
A class for handling time.
Definition: time.h:91
virtual void loop()
Code to execute in the thread.
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void finalize()
Finalize the thread.
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:235
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
Thread aspect to use blocked timing.
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:262
Shared memory image buffer.
Definition: shm_image.h:181
OpenNiDepthThread()
Constructor.
virtual ~OpenNiDepthThread()
Destructor.
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:714
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
void set_capture_time(fawkes::Time *time)
Set the capture time.
Definition: shm_image.cpp:205
virtual void init()
Initialize the thread.