23 #include "handtracker_thread.h"
25 #include "utils/conversions.h"
26 #include "utils/setup.h"
28 #include <core/threading/mutex_locker.h>
29 #include <interfaces/ObjectPositionInterface.h>
45 :
Thread(
"OpenNiHandTrackerThread",
Thread::OPMODE_WAITFORWAKEUP),
55 static void XN_CALLBACK_TYPE
56 cb_hand_create(xn::HandsGenerator &generator,
58 const XnPoint3D * position,
66 static void XN_CALLBACK_TYPE
67 cb_hand_destroy(xn::HandsGenerator &generator, XnUserID user, XnFloat time,
void *cookie)
73 static void XN_CALLBACK_TYPE
74 cb_hand_update(xn::HandsGenerator &generator,
76 const XnPoint3D * position,
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,
95 static void XN_CALLBACK_TYPE
96 cb_gesture_progress(xn::GestureGenerator &generator,
97 const XnChar * gesture_name,
98 const XnPoint3D * position,
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_);
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_);
122 gesture_gen_ =
new xn::GestureGenerator();
123 depth_gen_ =
new xn::DepthGenerator();
127 fawkes::openni::get_resolution(
config, width_, height_);
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_);
132 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_GESTURE, gesture_gen_);
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));
140 st = gesture_gen_->RegisterGestureCallbacks(cb_gesture_recognized,
144 if (st != XN_STATUS_OK) {
145 throw Exception(
"Failed to register gesture callbacks (%s)", xnGetStatusString(st));
150 for (
unsigned int i = 0; i < num_g; ++i) {
151 gest[i] =
new XnChar[64];
153 if ((st = gesture_gen_->EnumerateAllGestures(gest, 64, num_g)) != XN_STATUS_OK) {
156 for (
unsigned int i = 0; i < num_g; ++i) {
160 for (
unsigned int i = 0; i < num_g; ++i) {
165 gesture_gen_->AddGesture(
"Wave", NULL);
166 enabled_gesture_[
"Wave"] =
true;
168 gesture_gen_->AddGesture(
"Click", NULL);
169 enabled_gesture_[
"Click"] =
true;
171 hand_gen_->StartGenerating();
172 gesture_gen_->StartGenerating();
184 handgen_uniqueptr.release();
185 depthgen_uniqueptr.release();
186 gesturegen_uniqueptr.release();
193 for (i = hands_.begin(); i != hands_.end(); ++i) {
194 hand_gen_->StopTracking(i->first);
195 i->second->set_visible(
false);
201 std::map<std::string, bool>::iterator g;
202 for (g = enabled_gesture_.begin(); g != enabled_gesture_.end(); ++g) {
204 gesture_gen_->RemoveGesture(g->first.c_str());
217 if (!hand_gen_->IsDataNew())
221 for (i = hands_.begin(); i != hands_.end(); ++i) {
222 if (needs_write_[i->first]) {
224 needs_write_[i->first] =
false;
230 OpenNiHandTrackerThread::update_hand(XnUserID &user,
const XnPoint3D *position)
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);
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);
244 needs_write_[user] =
true;
260 if (hands_.find(user) != hands_.end()) {
266 if (asprintf(&ifid,
"OpenNI Hand %u", user) == -1) {
268 "New hand ID %u, but cannot generate "
276 update_hand(user, position);
293 if (hands_.find(user) == hands_.end()) {
298 update_hand(user, position);
309 if (hands_.find(user) == hands_.end()) {
316 hands_[user]->set_visible(
false);
317 hands_[user]->write();
319 logger->
log_error(
name(),
"Lost hand ID %u, closing interface '%s'", user, hands_[user]->uid());
322 needs_write_.erase(user);
325 std::map<std::string, bool>::iterator i;
326 for (i = enabled_gesture_.begin(); i != enabled_gesture_.end(); ++i) {
330 gesture_gen_->AddGesture(i->first.c_str(), NULL);
342 const XnPoint3D *position,
343 const XnPoint3D *end_position)
347 std::map<std::string, bool>::iterator i;
348 for (i = enabled_gesture_.begin(); i != enabled_gesture_.end(); ++i) {
352 gesture_gen_->RemoveGesture(i->first.c_str());
355 hand_gen_->StartTracking(*end_position);
365 const XnPoint3D *position,