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