Fawkes API  Fawkes Development Version
context_thread.cpp
00001 
00002 /***************************************************************************
00003  *  context_thread.cpp - OpenNI context providing Thread
00004  *
00005  *  Created: Sat Feb 26 17:46:29 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 "context_thread.h"
00024 #include "utils/version.h"
00025 
00026 #include <cerrno>
00027 #include <csignal>
00028 #include <unistd.h>
00029 #include <sys/wait.h>
00030 #include <XnCppWrapper.h>
00031 
00032 using namespace fawkes;
00033 
00034 /** @class OpenNiContextThread "context_thread.h"
00035  * OpenNI Context Thread.
00036  * This thread maintains an OpenNI context which can be used by other
00037  * threads and is provided via the OpenNiAspect.
00038  *
00039  * @author Tim Niemueller
00040  */
00041 
00042 /** Constructor. */
00043 OpenNiContextThread::OpenNiContextThread()
00044   : Thread("OpenNiContextThread", Thread::OPMODE_WAITFORWAKEUP),
00045     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
00046     AspectProviderAspect("OpenNiAspect", &__openni_aspect_inifin)
00047 {
00048 }
00049 
00050 
00051 /** Destructor. */
00052 OpenNiContextThread::~OpenNiContextThread()
00053 {
00054 }
00055 
00056 
00057 void
00058 OpenNiContextThread::init()
00059 {
00060   __sensor_server_pid = -1;
00061   __cfg_run_sensor_server = false;
00062   try {
00063     __cfg_run_sensor_server =
00064       config->get_bool("/plugins/openni/run_sensor_server");
00065   } catch (Exception &e) {} // ignore and use default
00066   if (__cfg_run_sensor_server) {
00067     __cfg_sensor_bin = config->get_string("/plugins/openni/sensor_server_bin");
00068   }
00069 
00070   __openni = new xn::Context();
00071 
00072   XnStatus st;
00073   if ((st = __openni->Init()) != XN_STATUS_OK) {
00074     __openni.clear();
00075     throw Exception("Initializing OpenNI failed: %s", xnGetStatusString(st));
00076   }
00077 
00078   __last_refcount = __openni.refcount();
00079 
00080   __check_now.set_clock(clock);
00081   __check_last.set_clock(clock);
00082   __check_last.stamp();
00083 
00084   __device_no_data_loops = 0;
00085   __openni_aspect_inifin.set_openni_context(__openni);
00086 
00087   if (__cfg_run_sensor_server) {
00088     start_sensor_server();
00089 
00090     // We don't want the server to die, we kill it by ourself.
00091     // Therefore we hold an instance all the time. Since client
00092     // connections are not properly terminated on unloading openni,
00093     // setting the timeout to 0 to stop the server immediately after
00094     // it is no longer used does not work.
00095     xn::NodeInfoList list;
00096     if (__openni->EnumerateProductionTrees(XN_NODE_TYPE_DEVICE, NULL, list)
00097         == XN_STATUS_OK)
00098     {
00099       for (xn::NodeInfoList::Iterator i = list.Begin(); i != list.End(); ++i) {
00100         if ((*i).GetDescription().Type == XN_NODE_TYPE_DEVICE) {
00101           __device = new xn::Device();
00102           (*i).GetInstance(*__device);
00103           break;
00104         }
00105       }
00106     }
00107   }
00108 }
00109 
00110 
00111 void
00112 OpenNiContextThread::finalize()
00113 {
00114   __openni->StopGeneratingAll();
00115 
00116 #if XN_VERSION_GE(1,3,2,0)
00117   __openni->Release();
00118 #else
00119   __openni->Shutdown();
00120 #endif
00121   __openni.clear();
00122   __openni_aspect_inifin.set_openni_context(__openni);
00123 
00124   if (__cfg_run_sensor_server) {
00125     delete __device;
00126     stop_sensor_server();
00127   }
00128 }
00129 
00130 
00131 void
00132 OpenNiContextThread::loop()
00133 {
00134   __openni.lock();
00135   if (__openni.refcount() != __last_refcount) {
00136     print_nodes();
00137     __last_refcount = __openni.refcount();
00138   }
00139   __openni->WaitNoneUpdateAll();
00140 
00141   __check_now.stamp();
00142   if ((__check_now - &__check_last) > 5) {
00143     verify_active();
00144     __check_last = __check_now;
00145   }
00146 
00147   __openni.unlock();
00148 }
00149 
00150 inline const char *
00151 type_to_string(XnProductionNodeType type)
00152 {
00153   switch (type) {
00154   case XN_NODE_TYPE_DEVICE:   return "device";
00155   case XN_NODE_TYPE_DEPTH:    return "depth";
00156   case XN_NODE_TYPE_IMAGE:    return "image";
00157   case XN_NODE_TYPE_AUDIO:    return "audio";
00158   case XN_NODE_TYPE_IR:       return "IR";
00159   case XN_NODE_TYPE_USER:     return "user";
00160   case XN_NODE_TYPE_RECORDER: return "recorder";
00161   case XN_NODE_TYPE_PLAYER:   return "player";
00162   case XN_NODE_TYPE_GESTURE:  return "gesture";
00163   case XN_NODE_TYPE_SCENE:    return "scene";
00164   case XN_NODE_TYPE_HANDS:    return "hands";
00165   case XN_NODE_TYPE_CODEC:    return "codec";
00166   default:                    return "unknown";
00167   }
00168 }
00169 
00170 /** Print active nodes to log.
00171  * Assumes that the context has been locked.
00172  */
00173 void
00174 OpenNiContextThread::print_nodes()
00175 {
00176   xn::NodeInfoList nodes;
00177   if (__openni->EnumerateExistingNodes(nodes) == XN_STATUS_OK) {
00178     logger->log_info(name(), "Currently existing nodes:");
00179     for (xn::NodeInfoList::Iterator n = nodes.Begin(); n != nodes.End(); ++n) {
00180       const XnProductionNodeDescription &pnd = (*n).GetDescription();
00181       const char *info = (*n).GetCreationInfo();
00182       if (strlen(info) == 0)  info = NULL;
00183 
00184       xn::Generator generator;
00185       bool have_gen = ((*n).GetInstance(generator) == XN_STATUS_OK);
00186 
00187       logger->log_info(name(), "  %-8s %8s (type: %-8s  vendor: %-12s  name: %-24s  "
00188                        "version: %u.%u.%u.%u%s%s)",
00189                        (*n).GetInstanceName(),
00190                        have_gen ? (generator.IsGenerating() ? "active" : "inactive") : "unknown",
00191                        type_to_string(pnd.Type), pnd.strVendor, pnd.strName,
00192                        pnd.Version.nMajor, pnd.Version.nMinor, pnd.Version.nMaintenance,
00193                        pnd.Version.nBuild, info ? "  info: " : "", info ? info : "");
00194     }
00195   }
00196 }
00197 
00198 
00199 /** Verify that all nodes are active.
00200  * Assumes that the context has been locked.
00201  */
00202 void
00203 OpenNiContextThread::verify_active()
00204 {
00205   xn::NodeInfoList nodes;
00206   if (__openni->EnumerateExistingNodes(nodes) == XN_STATUS_OK) {
00207     for (xn::NodeInfoList::Iterator n = nodes.Begin(); n != nodes.End(); ++n) {
00208       xn::Generator generator;
00209       bool have_gen = ((*n).GetInstance(generator) == XN_STATUS_OK);
00210 
00211       if (have_gen) {
00212         const XnProductionNodeDescription &pnd = (*n).GetDescription();
00213         // do not verify on device nodes for now, always reports inactive :-/
00214         if (pnd.Type != XN_NODE_TYPE_DEVICE) {
00215           if (! generator.IsGenerating()) {
00216             logger->log_warn(name(), "Inactive node '%s' (%s, %s/%s), trying to activate",
00217                              (*n).GetInstanceName(), type_to_string(pnd.Type),
00218                              pnd.strVendor, pnd.strName);
00219             generator.StartGenerating();
00220 
00221           } else if (! generator.IsDataNew()) {
00222             if (__dead_loops.find((*n).GetInstanceName()) != __dead_loops.end()) {
00223               __dead_loops[(*n).GetInstanceName()] += 1;
00224             } else {
00225               __dead_loops[(*n).GetInstanceName()]  = 1;
00226             }
00227 
00228           } else if (__dead_loops.find((*n).GetInstanceName()) != __dead_loops.end()) {
00229             __dead_loops.erase((*n).GetInstanceName());
00230           }
00231 
00232           /* The following does not work atm because IsDataNew() always reports false.
00233              While this could be because the WaitNoneUpdateAll() did not yet update the
00234              device node, event the timestamp does not change, therefore rendering this
00235              way to detect death of a device node unusable.
00236 
00237         } else if (pnd.Type == XN_NODE_TYPE_DEVICE) {
00238           // as an alternative, verify how often it has not been updated
00239           if (generator.IsDataNew()) {
00240             __device_no_data_loops = 0;
00241           } else {
00242             if (++__device_no_data_loops > 10) {
00243               logger->log_warn(name(), "Device '%s' had no fresh data for long time. "
00244                                "Reload maybe necessary.", (*n).GetInstanceName());
00245             }
00246           }
00247           */
00248         }
00249 
00250         xn::ErrorStateCapability ecap = generator.GetErrorStateCap();
00251         if (ecap.GetErrorState() != XN_STATUS_OK) {
00252           logger->log_warn(name(), "ERROR in node '%s': %s", (*n).GetInstanceName(),
00253                            xnGetStatusString(ecap.GetErrorState()));
00254         }
00255       }
00256     }
00257   }
00258 
00259   std::map<std::string, unsigned int>::iterator d;
00260   for (d = __dead_loops.begin(); d != __dead_loops.end(); ++d) {
00261     if (d->second >= 3) {
00262       logger->log_warn(name(), "Node '%s' had no fresh data for long time (%u tests)",
00263                        d->first.c_str(), d->second);
00264     }
00265   }
00266 }
00267 
00268 /** Start the sensor server daemon. */
00269 void
00270 OpenNiContextThread::start_sensor_server()
00271 {
00272   if (__sensor_server_pid != -1) {
00273     throw Exception("Sensor server appears to be already running");
00274   }
00275 
00276   logger->log_info(name(), "Starting XnSensorServer");
00277 
00278   pid_t pid = fork();
00279   if (pid == -1) {
00280     throw Exception(errno, "Forking for new process failed: %s");
00281   } else if (pid == 0) {
00282     // child
00283     setsid();
00284     // ignore SIGINT, we will propagate it as SIGTERM on unload
00285     signal(SIGINT, SIG_IGN);
00286     fclose(stdout);
00287     fclose(stdin);
00288     fclose(stderr);
00289     char *argv[] = {(char *)__cfg_sensor_bin.c_str(), NULL};
00290     if (execve(__cfg_sensor_bin.c_str(), argv, environ) == -1) {
00291       throw Exception("Failed to execute %s, exited with %i: %s\n",
00292                       __cfg_sensor_bin.c_str(), errno, strerror(errno));
00293     }
00294   }
00295 
00296   __sensor_server_pid = pid;
00297 }
00298 
00299 void
00300 OpenNiContextThread::stop_sensor_server()
00301 {
00302   if (__sensor_server_pid == -1) {
00303     throw Exception("Sensor server appears not to be already running");
00304   }
00305 
00306   logger->log_info(name(), "Stopping XnSensorServer");
00307   ::kill(__sensor_server_pid, SIGTERM);
00308   for (unsigned int i = 0; i < 200; ++i) {
00309     usleep(10000);
00310     int status;
00311     int rv = waitpid(__sensor_server_pid, &status, WNOHANG);
00312     if (rv == -1) {
00313       if (errno == EINTR)  continue;
00314       if (errno == ECHILD) {
00315         __sensor_server_pid = -1;
00316         break;
00317       }
00318     } else if (rv > 0) {
00319       __sensor_server_pid = -1;
00320       break;
00321     }
00322   }
00323 
00324   if (__sensor_server_pid != -1) {
00325     logger->log_warn(name(), "Killing XnSensorServer");
00326     ::kill(__sensor_server_pid, SIGKILL);
00327     __sensor_server_pid = -1;
00328   }
00329 }