Fawkes API  Fawkes Development Version
handtracker_thread.cpp
00001 
00002 /***************************************************************************
00003  *  handtracker_thread.cpp - OpenNI hand tracker thread
00004  *
00005  *  Created: Sun Feb 27 17:53:38 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "handtracker_thread.h"
00024 #include "utils/setup.h"
00025 #include "utils/conversions.h"
00026 
00027 #include <core/threading/mutex_locker.h>
00028 #include <interfaces/ObjectPositionInterface.h>
00029 
00030 #include <memory>
00031 
00032 using namespace fawkes;
00033 
00034 /** @class OpenNiHandTrackerThread "handtracker_thread.h"
00035  * OpenNI Hand Tracker Thread.
00036  * This thread requests a hand tracker node from OpenNI and publishes the
00037  * retrieved information via the blackboard.
00038  *
00039  * @author Tim Niemueller
00040  */
00041 
00042 /** Constructor. */
00043 OpenNiHandTrackerThread::OpenNiHandTrackerThread()
00044   : Thread("OpenNiHandTrackerThread", Thread::OPMODE_WAITFORWAKEUP),
00045     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
00046 {
00047 }
00048 
00049 
00050 /** Destructor. */
00051 OpenNiHandTrackerThread::~OpenNiHandTrackerThread()
00052 {
00053 }
00054 
00055 
00056 static void XN_CALLBACK_TYPE
00057 cb_hand_create(xn::HandsGenerator &generator, XnUserID user,
00058                const XnPoint3D *position, XnFloat time, void *cookie)
00059 {
00060   OpenNiHandTrackerThread *t = static_cast<OpenNiHandTrackerThread *>(cookie);
00061   t->hand_create(user, position, time);
00062 }
00063 
00064 static void XN_CALLBACK_TYPE
00065 cb_hand_destroy(xn::HandsGenerator &generator, XnUserID user,
00066                 XnFloat time, void *cookie)
00067 {
00068   OpenNiHandTrackerThread *t = static_cast<OpenNiHandTrackerThread *>(cookie);
00069   t->hand_destroy(user, time);
00070 }
00071 
00072 static void XN_CALLBACK_TYPE
00073 cb_hand_update(xn::HandsGenerator &generator, XnUserID user,
00074                const XnPoint3D *position, XnFloat time, void *cookie)
00075 {
00076   OpenNiHandTrackerThread *t = static_cast<OpenNiHandTrackerThread *>(cookie);
00077   t->hand_update(user, position, time);
00078 }
00079 
00080 
00081 static void XN_CALLBACK_TYPE
00082 cb_gesture_recognized(xn::GestureGenerator &generator,
00083                       const XnChar *gesture_name, const XnPoint3D *position,
00084                       const XnPoint3D *end_position, void *cookie)
00085 {
00086   OpenNiHandTrackerThread *t = static_cast<OpenNiHandTrackerThread *>(cookie);
00087   t->gesture_recognized(gesture_name, position, end_position);
00088 }
00089 
00090 static void XN_CALLBACK_TYPE
00091 cb_gesture_progress(xn::GestureGenerator &generator,
00092                     const XnChar *gesture_name, const XnPoint3D *position,
00093                     XnFloat progress, void *cookie)
00094 {
00095   OpenNiHandTrackerThread *t = static_cast<OpenNiHandTrackerThread *>(cookie);
00096   t->gesture_progress(gesture_name, position, progress);
00097 }
00098 
00099 
00100 void
00101 OpenNiHandTrackerThread::init()
00102 {
00103   MutexLocker lock(openni.objmutex_ptr());
00104 
00105   __hand_gen = new xn::HandsGenerator();
00106   std::auto_ptr<xn::HandsGenerator> handgen_autoptr(__hand_gen);
00107 
00108   __gesture_gen = new xn::GestureGenerator();
00109   std::auto_ptr<xn::GestureGenerator> gesturegen_autoptr(__gesture_gen);
00110 
00111   __depth_gen = new xn::DepthGenerator();
00112   std::auto_ptr<xn::DepthGenerator> depthgen_autoptr(__depth_gen);
00113 
00114   XnStatus st;
00115 
00116   fawkes::openni::get_resolution(config, __width, __height);
00117 
00118   fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_HANDS, __hand_gen);
00119   fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_DEPTH, __depth_gen);
00120   //fawkes::openni::setup_map_generator(*__depth_gen, config);
00121   fawkes::openni::find_or_create_node(openni, XN_NODE_TYPE_GESTURE, __gesture_gen);
00122 
00123   st = __hand_gen->RegisterHandCallbacks(cb_hand_create, cb_hand_update,
00124                                          cb_hand_destroy, this, __hand_cb_handle);
00125   if (st != XN_STATUS_OK) {
00126     throw Exception("Failed to register hand callbacks (%s)",
00127                     xnGetStatusString(st));
00128   }
00129 
00130   st = __gesture_gen->RegisterGestureCallbacks(cb_gesture_recognized,
00131                                                cb_gesture_progress,
00132                                                this, __gesture_cb_handle);
00133   if (st != XN_STATUS_OK) {
00134     throw Exception("Failed to register gesture callbacks (%s)",
00135                     xnGetStatusString(st));
00136   }
00137 
00138   XnUInt16 num_g = 64;
00139   XnChar *gest[64];
00140   for (unsigned int i = 0; i < num_g; ++i) {
00141     gest[i] = new XnChar[64];
00142   }
00143   if ((st = __gesture_gen->EnumerateAllGestures(gest, 64, num_g)) != XN_STATUS_OK)
00144   {
00145     logger->log_warn(name(), "Failed to enumerate gestures: %s",
00146                      xnGetStatusString(st));
00147   } else {
00148     for (unsigned int i = 0; i < num_g; ++i) {
00149       logger->log_debug(name(), "Supported gesture: %s", gest[i]);
00150 
00151     }
00152   }
00153   for (unsigned int i = 0; i < num_g; ++i) {
00154     delete[] gest[i];
00155   }
00156 
00157   logger->log_debug(name(), "Enabling gesture 'Wave'");
00158   __gesture_gen->AddGesture("Wave", NULL);
00159   __enabled_gesture["Wave"] = true;
00160   logger->log_debug(name(), "Enabling gesture 'Click'");
00161   __gesture_gen->AddGesture("Click", NULL);
00162   __enabled_gesture["Click"] = true;
00163 
00164   __hand_gen->StartGenerating();
00165   __gesture_gen->StartGenerating();
00166 
00167   // XnChar tmp[1000];
00168   // XnUInt64 tmpi;
00169   // //  if (__gesture_gen->GetIntProperty("AdaptiveDownscaleClosestVGA", tmpi) != XN_STATUS_OK) {
00170   // if ((st = __gesture_gen->GetStringProperty("Resolution", tmp, 1000)) == XN_STATUS_OK) {
00171   //   logger->log_debug(name(), "Resolution: %u", tmp);
00172   // } else {
00173   //   logger->log_debug(name(), "Failed to get resolution: %s",
00174   //                  xnGetStatusString(st));
00175   // }
00176 
00177   handgen_autoptr.release();
00178   depthgen_autoptr.release();
00179   gesturegen_autoptr.release();
00180 }
00181 
00182 
00183 void
00184 OpenNiHandTrackerThread::finalize()
00185 {
00186   HandMap::iterator i;
00187   for (i = __hands.begin(); i != __hands.end(); ++i) {
00188     __hand_gen->StopTracking(i->first);
00189     i->second->set_visible(false);
00190     i->second->set_valid(false);
00191     i->second->write();
00192     blackboard->close(i->second);
00193   }
00194   __hands.clear();
00195 
00196   std::map<std::string, bool>::iterator g;
00197   for (g = __enabled_gesture.begin(); g != __enabled_gesture.end(); ++g) {
00198     if (g->second) {
00199       __gesture_gen->RemoveGesture(g->first.c_str());
00200     }
00201   }
00202 
00203   // we do not stop generating, we don't know if there is no other plugin
00204   // using the node.
00205   delete __hand_gen;
00206   delete __gesture_gen;
00207 }
00208 
00209 
00210 void
00211 OpenNiHandTrackerThread::loop()
00212 {
00213   if (! __hand_gen->IsDataNew())  return;
00214 
00215   HandMap::iterator i;
00216   for (i = __hands.begin(); i != __hands.end(); ++i) {
00217     if (__needs_write[i->first]) {
00218       i->second->write();
00219       __needs_write[i->first] = false;
00220     }
00221   }
00222 }
00223 
00224 void
00225 OpenNiHandTrackerThread::update_hand(XnUserID &user, const XnPoint3D *position)
00226 {
00227   // convert to Fawkes coordinates
00228   __hands[user]->set_visible(true);
00229   __hands[user]->set_relative_x( position->Z * 0.001);
00230   __hands[user]->set_relative_y(-position->X * 0.001);
00231   __hands[user]->set_relative_z( position->Y * 0.001);
00232 
00233   XnPoint3D proj;
00234   fawkes::openni::world2projection(__depth_gen, 1, position, &proj,
00235                                    __width, __height);
00236   __hands[user]->set_world_x(proj.X);
00237   __hands[user]->set_world_y(proj.Y);
00238   __hands[user]->set_world_z(user);
00239 
00240   __needs_write[user] = true;
00241 
00242   //logger->log_debug(name(), "New hand pos: (%f,%f,%f)",
00243   //                __hands[user]->relative_x(), __hands[user]->relative_y(),
00244   //                __hands[user]->relative_z());
00245 }
00246 
00247 
00248 /** Notify of new hand.
00249  * This is called by the OpenNI callback when a new hand has been detected.
00250  * @param user new user's ID
00251  * @param position hand position
00252  * @param time timestamp in seconds
00253  */
00254 void
00255 OpenNiHandTrackerThread::hand_create(XnUserID &user, const XnPoint3D *position,
00256                                      XnFloat &time)
00257 {
00258   if (__hands.find(user) != __hands.end()) {
00259     logger->log_error(name(), "New hand ID %u, but interface already exists", user);
00260     return;
00261   }
00262 
00263   char *ifid;
00264   if (asprintf(&ifid, "OpenNI Hand %u", user) == -1) {
00265     logger->log_warn(name(), "New hand ID %u, but cannot generate "
00266                      "interface ID", user);
00267     return;
00268   }
00269   try {
00270     logger->log_debug(name(), "Opening interface 'ObjectPositionInterface::%s'",
00271                       ifid);
00272     __hands[user] = blackboard->open_for_writing<ObjectPositionInterface>(ifid);
00273     update_hand(user, position);
00274   } catch (Exception &e) {
00275     logger->log_warn(name(), "Failed to open interface, exception follows");
00276     logger->log_warn(name(), e);
00277   }
00278   free(ifid);
00279 }
00280 
00281 
00282 /** Notify of hand update.
00283  * This is called by the OpenNI callback when a new hand has been detected.
00284  * @param user new user's ID
00285  * @param position hand position
00286  * @param time timestamp in seconds
00287  */
00288 void
00289 OpenNiHandTrackerThread::hand_update(XnUserID &user, const XnPoint3D *position,
00290                                      XnFloat &time)
00291 {
00292   if (__hands.find(user) == __hands.end()) {
00293     logger->log_error(name(), "Got update for untracked hand %u", user);
00294     return;
00295   }
00296 
00297   update_hand(user, position);
00298 }
00299 
00300 
00301 /** Notify of disappeared hand.
00302  * This is called by the OpenNI callback when a new hand has been detected.
00303  * @param user new user's ID
00304  * @param time timestamp in seconds
00305  */
00306 void
00307 OpenNiHandTrackerThread::hand_destroy(XnUserID &user, XnFloat &time)
00308 {
00309   if (__hands.find(user) == __hands.end()) {
00310     logger->log_error(name(), "Got destroy for untracked hand %u", user);
00311     return;
00312   }
00313 
00314   //__hand_gen->StopTracking(user);
00315 
00316   __hands[user]->set_visible(false);
00317   __hands[user]->write();
00318 
00319   logger->log_error(name(), "Lost hand ID %u, closing interface '%s'",
00320                     user, __hands[user]->uid());
00321 
00322   blackboard->close(__hands[user]);
00323   __needs_write.erase(user);
00324   __hands.erase(user);
00325 
00326   std::map<std::string, bool>::iterator i;
00327   for (i = __enabled_gesture.begin(); i != __enabled_gesture.end(); ++i) {
00328     if (! i->second) {
00329       logger->log_debug(name(), "Enabling gesture '%s'", i->first.c_str());
00330       i->second = true;
00331       __gesture_gen->AddGesture(i->first.c_str(), NULL);
00332     }
00333   }
00334 }
00335 
00336 
00337 /** Notify of recognized gesture.
00338  * @param gesture_name name of the recognized gesture
00339  * @param position gesture position
00340  * @param end_position final hand position when completing the gesture
00341  */
00342 void
00343 OpenNiHandTrackerThread::gesture_recognized(const XnChar *gesture_name,
00344                                             const XnPoint3D *position,
00345                                             const XnPoint3D *end_position)
00346 {
00347   logger->log_debug(name(), "Gesture %s recognized, starting tracking",
00348                     gesture_name);
00349 
00350   std::map<std::string, bool>::iterator i;
00351   for (i = __enabled_gesture.begin(); i != __enabled_gesture.end(); ++i) {
00352     if (i->second) {
00353       logger->log_debug(name(), "Disabling gesture '%s'", i->first.c_str());
00354       i->second = false;
00355       __gesture_gen->RemoveGesture(i->first.c_str());
00356     }
00357   }
00358   __hand_gen->StartTracking(*end_position);
00359 }
00360 
00361 
00362 /** Notify of gesture progress.
00363  * @param gesture_name name of the recognized gesture
00364  * @param position gesture position
00365  * @param progress current progress of the recognition
00366  */
00367 void
00368 OpenNiHandTrackerThread::gesture_progress(const XnChar *gesture_name,
00369                                           const XnPoint3D *position,
00370                                           XnFloat progress)
00371 {
00372   logger->log_debug(name(), "Gesture %s progress %f", gesture_name, progress);
00373 }