Fawkes API  Fawkes Development Version
skel_if_observer.cpp
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