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
33using namespace fawkes;
34using 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
56void
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
97void
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
108void
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}
OpenNiDepthThread()
Constructor.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
virtual ~OpenNiDepthThread()
Destructor.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:284
Mutex locking helper.
Definition: mutex_locker.h:34
void unlock()
Unlock the mutex.
LockPtr< xn::Context > openni
Central OpenNI context.
Definition: openni.h:47
unsigned int num_attached() const
Get number of attached processes.
Definition: shm.cpp:763
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Time & stamp_systime()
Set this time to the current system time.
Definition: time.cpp:720
Shared memory image buffer.
Definition: shm_image.h:184
void set_capture_time(fawkes::Time *time)
Set the capture time.
Definition: shm_image.cpp:198
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:228
Fawkes library namespace.