Fawkes API Fawkes Development Version
handtracker_thread.cpp
1
2/***************************************************************************
3 * handtracker_thread.cpp - OpenNI hand tracker thread
4 *
5 * Created: Sun Feb 27 17:53:38 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 "handtracker_thread.h"
24
25#include "utils/conversions.h"
26#include "utils/setup.h"
27
28#include <core/threading/mutex_locker.h>
29#include <interfaces/ObjectPositionInterface.h>
30
31#include <memory>
32
33using namespace fawkes;
34
35/** @class OpenNiHandTrackerThread "handtracker_thread.h"
36 * OpenNI Hand Tracker Thread.
37 * This thread requests a hand tracker node from OpenNI and publishes the
38 * retrieved information via the blackboard.
39 *
40 * @author Tim Niemueller
41 */
42
43/** Constructor. */
45: Thread("OpenNiHandTrackerThread", Thread::OPMODE_WAITFORWAKEUP),
46 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
47{
48}
49
50/** Destructor. */
52{
53}
54
55static void XN_CALLBACK_TYPE
56cb_hand_create(xn::HandsGenerator &generator,
57 XnUserID user,
58 const XnPoint3D * position,
59 XnFloat time,
60 void * cookie)
61{
62 OpenNiHandTrackerThread *t = static_cast<OpenNiHandTrackerThread *>(cookie);
63 t->hand_create(user, position, time);
64}
65
66static void XN_CALLBACK_TYPE
67cb_hand_destroy(xn::HandsGenerator &generator, XnUserID user, XnFloat time, void *cookie)
68{
69 OpenNiHandTrackerThread *t = static_cast<OpenNiHandTrackerThread *>(cookie);
70 t->hand_destroy(user, time);
71}
72
73static void XN_CALLBACK_TYPE
74cb_hand_update(xn::HandsGenerator &generator,
75 XnUserID user,
76 const XnPoint3D * position,
77 XnFloat time,
78 void * cookie)
79{
80 OpenNiHandTrackerThread *t = static_cast<OpenNiHandTrackerThread *>(cookie);
81 t->hand_update(user, position, time);
82}
83
84static void XN_CALLBACK_TYPE
85cb_gesture_recognized(xn::GestureGenerator &generator,
86 const XnChar * gesture_name,
87 const XnPoint3D * position,
88 const XnPoint3D * end_position,
89 void * cookie)
90{
91 OpenNiHandTrackerThread *t = static_cast<OpenNiHandTrackerThread *>(cookie);
92 t->gesture_recognized(gesture_name, position, end_position);
93}
94
95static void XN_CALLBACK_TYPE
96cb_gesture_progress(xn::GestureGenerator &generator,
97 const XnChar * gesture_name,
98 const XnPoint3D * position,
99 XnFloat progress,
100 void * cookie)
101{
102 OpenNiHandTrackerThread *t = static_cast<OpenNiHandTrackerThread *>(cookie);
103 t->gesture_progress(gesture_name, position, progress);
104}
105
106void
108{
110
111 hand_gen_ = new xn::HandsGenerator();
112#if __cplusplus >= 201103L
113 std::unique_ptr<xn::HandsGenerator> handgen_uniqueptr(hand_gen_);
114 std::unique_ptr<xn::GestureGenerator> gesturegen_uniqueptr(gesture_gen_);
115 std::unique_ptr<xn::DepthGenerator> depthgen_uniqueptr(depth_gen_);
116#else
117 std::auto_ptr<xn::HandsGenerator> handgen_uniqueptr(hand_gen_);
118 std::auto_ptr<xn::GestureGenerator> gesturegen_uniqueptr(gesture_gen_);
119 std::auto_ptr<xn::DepthGenerator> depthgen_uniqueptr(depth_gen_);
120#endif
121
122 gesture_gen_ = new xn::GestureGenerator();
123 depth_gen_ = new xn::DepthGenerator();
124
125 XnStatus st;
126
127 fawkes::openni::get_resolution(config, width_, height_);
128
129 fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_HANDS, hand_gen_);
130 fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_DEPTH, depth_gen_);
131 //fawkes::openni::setup_map_generator(*depth_gen_, config);
132 fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_GESTURE, gesture_gen_);
133
134 st = hand_gen_->RegisterHandCallbacks(
135 cb_hand_create, cb_hand_update, cb_hand_destroy, this, hand_cb_handle_);
136 if (st != XN_STATUS_OK) {
137 throw Exception("Failed to register hand callbacks (%s)", xnGetStatusString(st));
138 }
139
140 st = gesture_gen_->RegisterGestureCallbacks(cb_gesture_recognized,
141 cb_gesture_progress,
142 this,
143 gesture_cb_handle_);
144 if (st != XN_STATUS_OK) {
145 throw Exception("Failed to register gesture callbacks (%s)", xnGetStatusString(st));
146 }
147
148 XnUInt16 num_g = 64;
149 XnChar * gest[64];
150 for (unsigned int i = 0; i < num_g; ++i) {
151 gest[i] = new XnChar[64];
152 }
153 if ((st = gesture_gen_->EnumerateAllGestures(gest, 64, num_g)) != XN_STATUS_OK) {
154 logger->log_warn(name(), "Failed to enumerate gestures: %s", xnGetStatusString(st));
155 } else {
156 for (unsigned int i = 0; i < num_g; ++i) {
157 logger->log_debug(name(), "Supported gesture: %s", gest[i]);
158 }
159 }
160 for (unsigned int i = 0; i < num_g; ++i) {
161 delete[] gest[i];
162 }
163
164 logger->log_debug(name(), "Enabling gesture 'Wave'");
165 gesture_gen_->AddGesture("Wave", NULL);
166 enabled_gesture_["Wave"] = true;
167 logger->log_debug(name(), "Enabling gesture 'Click'");
168 gesture_gen_->AddGesture("Click", NULL);
169 enabled_gesture_["Click"] = true;
170
171 hand_gen_->StartGenerating();
172 gesture_gen_->StartGenerating();
173
174 // XnChar tmp[1000];
175 // XnUInt64 tmpi;
176 // // if (gesture_gen_->GetIntProperty("AdaptiveDownscaleClosestVGA", tmpi) != XN_STATUS_OK) {
177 // if ((st = gesture_gen_->GetStringProperty("Resolution", tmp, 1000)) == XN_STATUS_OK) {
178 // logger->log_debug(name(), "Resolution: %u", tmp);
179 // } else {
180 // logger->log_debug(name(), "Failed to get resolution: %s",
181 // xnGetStatusString(st));
182 // }
183
184 handgen_uniqueptr.release();
185 depthgen_uniqueptr.release();
186 gesturegen_uniqueptr.release();
187}
188
189void
191{
192 HandMap::iterator i;
193 for (i = hands_.begin(); i != hands_.end(); ++i) {
194 hand_gen_->StopTracking(i->first);
195 i->second->set_visible(false);
196 i->second->write();
197 blackboard->close(i->second);
198 }
199 hands_.clear();
200
201 std::map<std::string, bool>::iterator g;
202 for (g = enabled_gesture_.begin(); g != enabled_gesture_.end(); ++g) {
203 if (g->second) {
204 gesture_gen_->RemoveGesture(g->first.c_str());
205 }
206 }
207
208 // we do not stop generating, we don't know if there is no other plugin
209 // using the node.
210 delete hand_gen_;
211 delete gesture_gen_;
212}
213
214void
216{
217 if (!hand_gen_->IsDataNew())
218 return;
219
220 HandMap::iterator i;
221 for (i = hands_.begin(); i != hands_.end(); ++i) {
222 if (needs_write_[i->first]) {
223 i->second->write();
224 needs_write_[i->first] = false;
225 }
226 }
227}
228
229void
230OpenNiHandTrackerThread::update_hand(XnUserID &user, const XnPoint3D *position)
231{
232 // convert to Fawkes coordinates
233 hands_[user]->set_visible(true);
234 hands_[user]->set_relative_x(position->Z * 0.001);
235 hands_[user]->set_relative_y(-position->X * 0.001);
236 hands_[user]->set_relative_z(position->Y * 0.001);
237
238 XnPoint3D proj;
239 fawkes::openni::world2projection(depth_gen_, 1, position, &proj, width_, height_);
240 hands_[user]->set_world_x(proj.X);
241 hands_[user]->set_world_y(proj.Y);
242 hands_[user]->set_world_z(user);
243
244 needs_write_[user] = true;
245
246 //logger->log_debug(name(), "New hand pos: (%f,%f,%f)",
247 // hands_[user]->relative_x(), hands_[user]->relative_y(),
248 // hands_[user]->relative_z());
249}
250
251/** Notify of new hand.
252 * This is called by the OpenNI callback when a new hand has been detected.
253 * @param user new user's ID
254 * @param position hand position
255 * @param time timestamp in seconds
256 */
257void
258OpenNiHandTrackerThread::hand_create(XnUserID &user, const XnPoint3D *position, XnFloat &time)
259{
260 if (hands_.find(user) != hands_.end()) {
261 logger->log_error(name(), "New hand ID %u, but interface already exists", user);
262 return;
263 }
264
265 char *ifid;
266 if (asprintf(&ifid, "OpenNI Hand %u", user) == -1) {
268 "New hand ID %u, but cannot generate "
269 "interface ID",
270 user);
271 return;
272 }
273 try {
274 logger->log_debug(name(), "Opening interface 'ObjectPositionInterface::%s'", ifid);
276 update_hand(user, position);
277 } catch (Exception &e) {
278 logger->log_warn(name(), "Failed to open interface, exception follows");
279 logger->log_warn(name(), e);
280 }
281 free(ifid);
282}
283
284/** Notify of hand update.
285 * This is called by the OpenNI callback when a new hand has been detected.
286 * @param user new user's ID
287 * @param position hand position
288 * @param time timestamp in seconds
289 */
290void
291OpenNiHandTrackerThread::hand_update(XnUserID &user, const XnPoint3D *position, XnFloat &time)
292{
293 if (hands_.find(user) == hands_.end()) {
294 logger->log_error(name(), "Got update for untracked hand %u", user);
295 return;
296 }
297
298 update_hand(user, position);
299}
300
301/** Notify of disappeared hand.
302 * This is called by the OpenNI callback when a new hand has been detected.
303 * @param user new user's ID
304 * @param time timestamp in seconds
305 */
306void
307OpenNiHandTrackerThread::hand_destroy(XnUserID &user, XnFloat &time)
308{
309 if (hands_.find(user) == hands_.end()) {
310 logger->log_error(name(), "Got destroy for untracked hand %u", user);
311 return;
312 }
313
314 //hand_gen_->StopTracking(user);
315
316 hands_[user]->set_visible(false);
317 hands_[user]->write();
318
319 logger->log_error(name(), "Lost hand ID %u, closing interface '%s'", user, hands_[user]->uid());
320
321 blackboard->close(hands_[user]);
322 needs_write_.erase(user);
323 hands_.erase(user);
324
325 std::map<std::string, bool>::iterator i;
326 for (i = enabled_gesture_.begin(); i != enabled_gesture_.end(); ++i) {
327 if (!i->second) {
328 logger->log_debug(name(), "Enabling gesture '%s'", i->first.c_str());
329 i->second = true;
330 gesture_gen_->AddGesture(i->first.c_str(), NULL);
331 }
332 }
333}
334
335/** Notify of recognized gesture.
336 * @param gesture_name name of the recognized gesture
337 * @param position gesture position
338 * @param end_position final hand position when completing the gesture
339 */
340void
342 const XnPoint3D *position,
343 const XnPoint3D *end_position)
344{
345 logger->log_debug(name(), "Gesture %s recognized, starting tracking", gesture_name);
346
347 std::map<std::string, bool>::iterator i;
348 for (i = enabled_gesture_.begin(); i != enabled_gesture_.end(); ++i) {
349 if (i->second) {
350 logger->log_debug(name(), "Disabling gesture '%s'", i->first.c_str());
351 i->second = false;
352 gesture_gen_->RemoveGesture(i->first.c_str());
353 }
354 }
355 hand_gen_->StartTracking(*end_position);
356}
357
358/** Notify of gesture progress.
359 * @param gesture_name name of the recognized gesture
360 * @param position gesture position
361 * @param progress current progress of the recognition
362 */
363void
365 const XnPoint3D *position,
366 XnFloat progress)
367{
368 logger->log_debug(name(), "Gesture %s progress %f", gesture_name, progress);
369}
OpenNI Hand Tracker Thread.
void hand_create(XnUserID &user, const XnPoint3D *position, XnFloat &time)
Notify of new hand.
virtual void init()
Initialize the thread.
void hand_update(XnUserID &user, const XnPoint3D *position, XnFloat &time)
Notify of hand update.
void hand_destroy(XnUserID &user, XnFloat &time)
Notify of disappeared hand.
virtual void finalize()
Finalize the thread.
void gesture_recognized(const XnChar *gesture_name, const XnPoint3D *position, const XnPoint3D *end_position)
Notify of recognized gesture.
virtual ~OpenNiHandTrackerThread()
Destructor.
void gesture_progress(const XnChar *gesture_name, const XnPoint3D *position, XnFloat progress)
Notify of gesture progress.
OpenNiHandTrackerThread()
Constructor.
virtual void loop()
Code to execute in the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
Base class for exceptions in Fawkes.
Definition: exception.h:36
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:284
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Mutex locking helper.
Definition: mutex_locker.h:34
ObjectPositionInterface Fawkes BlackBoard Interface.
LockPtr< xn::Context > openni
Central OpenNI context.
Definition: openni.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.