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