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  std::auto_ptr<xn::HandsGenerator> handgen_autoptr(__hand_gen);
107 
108  __gesture_gen = new xn::GestureGenerator();
109  std::auto_ptr<xn::GestureGenerator> gesturegen_autoptr(__gesture_gen);
110 
111  __depth_gen = new xn::DepthGenerator();
112  std::auto_ptr<xn::DepthGenerator> depthgen_autoptr(__depth_gen);
113 
114  XnStatus st;
115 
116  fawkes::openni::get_resolution(config, __width, __height);
117 
118  fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_HANDS, __hand_gen);
119  fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_DEPTH, __depth_gen);
120  //fawkes::openni::setup_map_generator(*__depth_gen, config);
121  fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_GESTURE, __gesture_gen);
122 
123  st = __hand_gen->RegisterHandCallbacks(cb_hand_create, cb_hand_update,
124  cb_hand_destroy, this, __hand_cb_handle);
125  if (st != XN_STATUS_OK) {
126  throw Exception("Failed to register hand callbacks (%s)",
127  xnGetStatusString(st));
128  }
129 
130  st = __gesture_gen->RegisterGestureCallbacks(cb_gesture_recognized,
131  cb_gesture_progress,
132  this, __gesture_cb_handle);
133  if (st != XN_STATUS_OK) {
134  throw Exception("Failed to register gesture callbacks (%s)",
135  xnGetStatusString(st));
136  }
137 
138  XnUInt16 num_g = 64;
139  XnChar *gest[64];
140  for (unsigned int i = 0; i < num_g; ++i) {
141  gest[i] = new XnChar[64];
142  }
143  if ((st = __gesture_gen->EnumerateAllGestures(gest, 64, num_g)) != XN_STATUS_OK)
144  {
145  logger->log_warn(name(), "Failed to enumerate gestures: %s",
146  xnGetStatusString(st));
147  } else {
148  for (unsigned int i = 0; i < num_g; ++i) {
149  logger->log_debug(name(), "Supported gesture: %s", gest[i]);
150 
151  }
152  }
153  for (unsigned int i = 0; i < num_g; ++i) {
154  delete[] gest[i];
155  }
156 
157  logger->log_debug(name(), "Enabling gesture 'Wave'");
158  __gesture_gen->AddGesture("Wave", NULL);
159  __enabled_gesture["Wave"] = true;
160  logger->log_debug(name(), "Enabling gesture 'Click'");
161  __gesture_gen->AddGesture("Click", NULL);
162  __enabled_gesture["Click"] = true;
163 
164  __hand_gen->StartGenerating();
165  __gesture_gen->StartGenerating();
166 
167  // XnChar tmp[1000];
168  // XnUInt64 tmpi;
169  // // if (__gesture_gen->GetIntProperty("AdaptiveDownscaleClosestVGA", tmpi) != XN_STATUS_OK) {
170  // if ((st = __gesture_gen->GetStringProperty("Resolution", tmp, 1000)) == XN_STATUS_OK) {
171  // logger->log_debug(name(), "Resolution: %u", tmp);
172  // } else {
173  // logger->log_debug(name(), "Failed to get resolution: %s",
174  // xnGetStatusString(st));
175  // }
176 
177  handgen_autoptr.release();
178  depthgen_autoptr.release();
179  gesturegen_autoptr.release();
180 }
181 
182 
183 void
185 {
186  HandMap::iterator i;
187  for (i = __hands.begin(); i != __hands.end(); ++i) {
188  __hand_gen->StopTracking(i->first);
189  i->second->set_visible(false);
190  i->second->set_valid(false);
191  i->second->write();
192  blackboard->close(i->second);
193  }
194  __hands.clear();
195 
196  std::map<std::string, bool>::iterator g;
197  for (g = __enabled_gesture.begin(); g != __enabled_gesture.end(); ++g) {
198  if (g->second) {
199  __gesture_gen->RemoveGesture(g->first.c_str());
200  }
201  }
202 
203  // we do not stop generating, we don't know if there is no other plugin
204  // using the node.
205  delete __hand_gen;
206  delete __gesture_gen;
207 }
208 
209 
210 void
212 {
213  if (! __hand_gen->IsDataNew()) return;
214 
215  HandMap::iterator i;
216  for (i = __hands.begin(); i != __hands.end(); ++i) {
217  if (__needs_write[i->first]) {
218  i->second->write();
219  __needs_write[i->first] = false;
220  }
221  }
222 }
223 
224 void
225 OpenNiHandTrackerThread::update_hand(XnUserID &user, const XnPoint3D *position)
226 {
227  // convert to Fawkes coordinates
228  __hands[user]->set_visible(true);
229  __hands[user]->set_relative_x( position->Z * 0.001);
230  __hands[user]->set_relative_y(-position->X * 0.001);
231  __hands[user]->set_relative_z( position->Y * 0.001);
232 
233  XnPoint3D proj;
234  fawkes::openni::world2projection(__depth_gen, 1, position, &proj,
235  __width, __height);
236  __hands[user]->set_world_x(proj.X);
237  __hands[user]->set_world_y(proj.Y);
238  __hands[user]->set_world_z(user);
239 
240  __needs_write[user] = true;
241 
242  //logger->log_debug(name(), "New hand pos: (%f,%f,%f)",
243  // __hands[user]->relative_x(), __hands[user]->relative_y(),
244  // __hands[user]->relative_z());
245 }
246 
247 
248 /** Notify of new hand.
249  * This is called by the OpenNI callback when a new hand has been detected.
250  * @param user new user's ID
251  * @param position hand position
252  * @param time timestamp in seconds
253  */
254 void
255 OpenNiHandTrackerThread::hand_create(XnUserID &user, const XnPoint3D *position,
256  XnFloat &time)
257 {
258  if (__hands.find(user) != __hands.end()) {
259  logger->log_error(name(), "New hand ID %u, but interface already exists", user);
260  return;
261  }
262 
263  char *ifid;
264  if (asprintf(&ifid, "OpenNI Hand %u", user) == -1) {
265  logger->log_warn(name(), "New hand ID %u, but cannot generate "
266  "interface ID", user);
267  return;
268  }
269  try {
270  logger->log_debug(name(), "Opening interface 'ObjectPositionInterface::%s'",
271  ifid);
272  __hands[user] = blackboard->open_for_writing<ObjectPositionInterface>(ifid);
273  update_hand(user, position);
274  } catch (Exception &e) {
275  logger->log_warn(name(), "Failed to open interface, exception follows");
276  logger->log_warn(name(), e);
277  }
278  free(ifid);
279 }
280 
281 
282 /** Notify of hand update.
283  * This is called by the OpenNI callback when a new hand has been detected.
284  * @param user new user's ID
285  * @param position hand position
286  * @param time timestamp in seconds
287  */
288 void
289 OpenNiHandTrackerThread::hand_update(XnUserID &user, const XnPoint3D *position,
290  XnFloat &time)
291 {
292  if (__hands.find(user) == __hands.end()) {
293  logger->log_error(name(), "Got update for untracked hand %u", user);
294  return;
295  }
296 
297  update_hand(user, position);
298 }
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  */
306 void
307 OpenNiHandTrackerThread::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'",
320  user, __hands[user]->uid());
321 
322  blackboard->close(__hands[user]);
323  __needs_write.erase(user);
324  __hands.erase(user);
325 
326  std::map<std::string, bool>::iterator i;
327  for (i = __enabled_gesture.begin(); i != __enabled_gesture.end(); ++i) {
328  if (! i->second) {
329  logger->log_debug(name(), "Enabling gesture '%s'", i->first.c_str());
330  i->second = true;
331  __gesture_gen->AddGesture(i->first.c_str(), NULL);
332  }
333  }
334 }
335 
336 
337 /** Notify of recognized gesture.
338  * @param gesture_name name of the recognized gesture
339  * @param position gesture position
340  * @param end_position final hand position when completing the gesture
341  */
342 void
344  const XnPoint3D *position,
345  const XnPoint3D *end_position)
346 {
347  logger->log_debug(name(), "Gesture %s recognized, starting tracking",
348  gesture_name);
349 
350  std::map<std::string, bool>::iterator i;
351  for (i = __enabled_gesture.begin(); i != __enabled_gesture.end(); ++i) {
352  if (i->second) {
353  logger->log_debug(name(), "Disabling gesture '%s'", i->first.c_str());
354  i->second = false;
355  __gesture_gen->RemoveGesture(i->first.c_str());
356  }
357  }
358  __hand_gen->StartTracking(*end_position);
359 }
360 
361 
362 /** Notify of gesture progress.
363  * @param gesture_name name of the recognized gesture
364  * @param position gesture position
365  * @param progress current progress of the recognition
366  */
367 void
368 OpenNiHandTrackerThread::gesture_progress(const XnChar *gesture_name,
369  const XnPoint3D *position,
370  XnFloat progress)
371 {
372  logger->log_debug(name(), "Gesture %s progress %f", gesture_name, progress);
373 }
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 * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:240
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
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void gesture_recognized(const XnChar *gesture_name, const XnPoint3D *position, const XnPoint3D *end_position)
Notify of recognized gesture.
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.
const char * name() const
Get name of thread.
Definition: thread.h:95
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
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:43
virtual void close(Interface *interface)=0
Close interface.