Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * skel_if_observer.cpp - Skeleton interface observer 00004 * 00005 * Created: Sat Apr 02 18:20:29 2011 (RoboCup German Open 2011, Magdeburg) 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 <plugins/openni/utils/skel_if_observer.h> 00024 00025 #include <blackboard/blackboard.h> 00026 #include <interfaces/HumanSkeletonInterface.h> 00027 #include <interfaces/HumanSkeletonProjectionInterface.h> 00028 00029 #include <cstdio> 00030 00031 namespace fawkes { 00032 namespace openni { 00033 #if 0 /* just to make Emacs auto-indent happy */ 00034 } 00035 } 00036 #endif 00037 00038 /** @class SkelIfObserver <plugins/openni/utils/skel_if_observer.h> 00039 * Skeleton interface observer. 00040 * This class opens all OpenNI skeleton interfaces and registers as an 00041 * observer to open any newly opened interface. 00042 * @author Tim Niemueller 00043 */ 00044 00045 /** Constructor. 00046 * @param bb blackboard to interact with 00047 * @param users user map for exchange with others 00048 */ 00049 SkelIfObserver::SkelIfObserver(BlackBoard *bb, UserMap &users) 00050 : __users(users) 00051 { 00052 __queue_lock = new Mutex(); 00053 __bb = bb; 00054 00055 std::list<HumanSkeletonInterface *> skels = 00056 __bb->open_multiple_for_reading<HumanSkeletonInterface>("OpenNI Human *"); 00057 00058 std::list<HumanSkeletonProjectionInterface *> projs; 00059 00060 std::list<HumanSkeletonInterface *>::iterator i; 00061 for (i = skels.begin(); i != skels.end(); ++i) { 00062 printf("Opened %s\n", (*i)->uid()); 00063 00064 UserInfo user; 00065 user.skel_if = *i; 00066 user.proj_if = 00067 __bb->open_for_reading<HumanSkeletonProjectionInterface>(user.skel_if->id()); 00068 00069 __users[user.skel_if->id()] = user; 00070 } 00071 00072 bbio_add_observed_create("HumanSkeletonInterface", "OpenNI Human *"); 00073 __bb->register_observer(this); 00074 } 00075 00076 00077 /** Destructor. */ 00078 SkelIfObserver::~SkelIfObserver() 00079 { 00080 __bb->unregister_observer(this); 00081 delete __queue_lock; 00082 } 00083 00084 void 00085 SkelIfObserver::bb_interface_created(const char *type, const char *id) throw() 00086 { 00087 if (__users.find(id) == __users.end()) { 00088 __queue_lock->lock(); 00089 __queues[__active_queue].push(id); 00090 __queue_lock->unlock(); 00091 } 00092 } 00093 00094 /** Process internal queue. 00095 * This should be called regularly to process incoming events. 00096 */ 00097 void 00098 SkelIfObserver::process_queue() 00099 { 00100 __queue_lock->lock(); 00101 unsigned int proc_queue = __active_queue; 00102 __active_queue = 1 - __active_queue; 00103 __queue_lock->unlock(); 00104 while (! __queues[proc_queue].empty()) { 00105 std::string id = __queues[proc_queue].front(); 00106 00107 try { 00108 UserInfo user; 00109 printf("Opening %s\n", id.c_str()); 00110 user.skel_if = __bb->open_for_reading<HumanSkeletonInterface>(id.c_str()); 00111 try { 00112 user.proj_if = 00113 __bb->open_for_reading<HumanSkeletonProjectionInterface>(id.c_str()); 00114 } catch (Exception &e) { 00115 __bb->close(user.skel_if); 00116 throw; 00117 } 00118 00119 __users[id] = user; 00120 } catch (Exception &e) { 00121 e.print_trace(); 00122 continue; 00123 } 00124 00125 __queues[proc_queue].pop(); 00126 } 00127 } 00128 00129 } // end namespace fawkes::openni 00130 } // end namespace fawkes