23 #include "handtracker_thread.h"
24 #include "utils/setup.h"
25 #include "utils/conversions.h"
27 #include <core/threading/mutex_locker.h>
28 #include <interfaces/ObjectPositionInterface.h>
44 :
Thread(
"OpenNiHandTrackerThread",
Thread::OPMODE_WAITFORWAKEUP),
56 static void XN_CALLBACK_TYPE
57 cb_hand_create(xn::HandsGenerator &generator, XnUserID user,
58 const XnPoint3D *position, XnFloat time,
void *cookie)
64 static void XN_CALLBACK_TYPE
65 cb_hand_destroy(xn::HandsGenerator &generator, XnUserID user,
66 XnFloat time,
void *cookie)
72 static void XN_CALLBACK_TYPE
73 cb_hand_update(xn::HandsGenerator &generator, XnUserID user,
74 const XnPoint3D *position, XnFloat time,
void *cookie)
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)
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)
105 __hand_gen =
new xn::HandsGenerator();
106 std::auto_ptr<xn::HandsGenerator> handgen_autoptr(__hand_gen);
108 __gesture_gen =
new xn::GestureGenerator();
109 std::auto_ptr<xn::GestureGenerator> gesturegen_autoptr(__gesture_gen);
111 __depth_gen =
new xn::DepthGenerator();
112 std::auto_ptr<xn::DepthGenerator> depthgen_autoptr(__depth_gen);
116 fawkes::openni::get_resolution(
config, __width, __height);
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);
121 fawkes::openni::find_or_create_node(
openni, XN_NODE_TYPE_GESTURE, __gesture_gen);
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));
130 st = __gesture_gen->RegisterGestureCallbacks(cb_gesture_recognized,
132 this, __gesture_cb_handle);
133 if (st != XN_STATUS_OK) {
134 throw Exception(
"Failed to register gesture callbacks (%s)",
135 xnGetStatusString(st));
140 for (
unsigned int i = 0; i < num_g; ++i) {
141 gest[i] =
new XnChar[64];
143 if ((st = __gesture_gen->EnumerateAllGestures(gest, 64, num_g)) != XN_STATUS_OK)
146 xnGetStatusString(st));
148 for (
unsigned int i = 0; i < num_g; ++i) {
153 for (
unsigned int i = 0; i < num_g; ++i) {
158 __gesture_gen->AddGesture(
"Wave", NULL);
159 __enabled_gesture[
"Wave"] =
true;
161 __gesture_gen->AddGesture(
"Click", NULL);
162 __enabled_gesture[
"Click"] =
true;
164 __hand_gen->StartGenerating();
165 __gesture_gen->StartGenerating();
177 handgen_autoptr.release();
178 depthgen_autoptr.release();
179 gesturegen_autoptr.release();
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);
196 std::map<std::string, bool>::iterator g;
197 for (g = __enabled_gesture.begin(); g != __enabled_gesture.end(); ++g) {
199 __gesture_gen->RemoveGesture(g->first.c_str());
206 delete __gesture_gen;
213 if (! __hand_gen->IsDataNew())
return;
216 for (i = __hands.begin(); i != __hands.end(); ++i) {
217 if (__needs_write[i->first]) {
219 __needs_write[i->first] =
false;
225 OpenNiHandTrackerThread::update_hand(XnUserID &user,
const XnPoint3D *position)
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);
234 fawkes::openni::world2projection(__depth_gen, 1, position, &proj,
236 __hands[user]->set_world_x(proj.X);
237 __hands[user]->set_world_y(proj.Y);
238 __hands[user]->set_world_z(user);
240 __needs_write[user] =
true;
258 if (__hands.find(user) != __hands.end()) {
264 if (asprintf(&ifid,
"OpenNI Hand %u", user) == -1) {
266 "interface ID", user);
273 update_hand(user, position);
292 if (__hands.find(user) == __hands.end()) {
297 update_hand(user, position);
309 if (__hands.find(user) == __hands.end()) {
316 __hands[user]->set_visible(
false);
317 __hands[user]->write();
320 user, __hands[user]->uid());
323 __needs_write.erase(user);
326 std::map<std::string, bool>::iterator i;
327 for (i = __enabled_gesture.begin(); i != __enabled_gesture.end(); ++i) {
331 __gesture_gen->AddGesture(i->first.c_str(), NULL);
344 const XnPoint3D *position,
345 const XnPoint3D *end_position)
350 std::map<std::string, bool>::iterator i;
351 for (i = __enabled_gesture.begin(); i != __enabled_gesture.end(); ++i) {
355 __gesture_gen->RemoveGesture(i->first.c_str());
358 __hand_gen->StartTracking(*end_position);
369 const XnPoint3D *position,
LockPtr< xn::Context > openni
Central OpenNI context.
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.
virtual ~OpenNiHandTrackerThread()
Destructor.
OpenNI Hand Tracker Thread.
Thread class encapsulation of pthreads.
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.
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.
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.
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.
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.
virtual void close(Interface *interface)=0
Close interface.