Fawkes API
Fawkes Development Version
|
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 }