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 
33 using 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 
55 static void XN_CALLBACK_TYPE
56  cb_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 
66 static void XN_CALLBACK_TYPE
67  cb_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 
73 static void XN_CALLBACK_TYPE
74  cb_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 
84 static void XN_CALLBACK_TYPE
85  cb_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 
95 static void XN_CALLBACK_TYPE
96  cb_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 
106 void
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 
189 void
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 
214 void
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 
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, 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  */
257 void
258 OpenNiHandTrackerThread::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) {
267  logger->log_warn(name(),
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);
275  hands_[user] = blackboard->open_for_writing<ObjectPositionInterface>(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  */
290 void
291 OpenNiHandTrackerThread::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  */
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'", 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  */
340 void
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  */
363 void
364 OpenNiHandTrackerThread::gesture_progress(const XnChar * gesture_name,
365  const XnPoint3D *position,
366  XnFloat progress)
367 {
368  logger->log_debug(name(), "Gesture %s progress %f", gesture_name, progress);
369 }
LockPtr< xn::Context > openni
Central OpenNI context.
Definition: openni.h:47
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:45
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:41
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:35
Mutex * objmutex_ptr() const
Get object mutex.
Definition: lockptr.h:284
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:100
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:41
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.