Fawkes API  Fawkes Development Version
context_thread.cpp
1 
2 /***************************************************************************
3  * context_thread.cpp - OpenNI context providing Thread
4  *
5  * Created: Sat Feb 26 17:46:29 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "context_thread.h"
24 #include "utils/version.h"
25 
26 #include <cerrno>
27 #include <csignal>
28 #include <unistd.h>
29 #include <sys/wait.h>
30 #include <XnCppWrapper.h>
31 
32 using namespace fawkes;
33 
34 /** @class OpenNiContextThread "context_thread.h"
35  * OpenNI Context Thread.
36  * This thread maintains an OpenNI context which can be used by other
37  * threads and is provided via the OpenNiAspect.
38  *
39  * @author Tim Niemueller
40  */
41 
42 /** Constructor. */
44  : Thread("OpenNiContextThread", Thread::OPMODE_WAITFORWAKEUP),
45  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
46  AspectProviderAspect(&__openni_aspect_inifin)
47 {
48 }
49 
50 
51 /** Destructor. */
53 {
54 }
55 
56 
57 void
59 {
60  __sensor_server_pid = -1;
61  __cfg_run_sensor_server = false;
62  try {
63  __cfg_run_sensor_server =
64  config->get_bool("/plugins/openni/run_sensor_server");
65  } catch (Exception &e) {} // ignore and use default
66  if (__cfg_run_sensor_server) {
67  __cfg_sensor_bin = config->get_string("/plugins/openni/sensor_server_bin");
68  }
69 
70  __openni = new xn::Context();
71 
72  XnStatus st;
73  if ((st = __openni->Init()) != XN_STATUS_OK) {
74  __openni.clear();
75  throw Exception("Initializing OpenNI failed: %s", xnGetStatusString(st));
76  }
77 
78  __last_refcount = __openni.refcount();
79 
80  __check_now.set_clock(clock);
81  __check_last.set_clock(clock);
82  __check_last.stamp();
83 
84  __device_no_data_loops = 0;
85  __openni_aspect_inifin.set_openni_context(__openni);
86 
87  if (__cfg_run_sensor_server) {
88  start_sensor_server();
89 
90  // We don't want the server to die, we kill it by ourself.
91  // Therefore we hold an instance all the time. Since client
92  // connections are not properly terminated on unloading openni,
93  // setting the timeout to 0 to stop the server immediately after
94  // it is no longer used does not work.
95  xn::NodeInfoList list;
96  if (__openni->EnumerateProductionTrees(XN_NODE_TYPE_DEVICE, NULL, list)
97  == XN_STATUS_OK)
98  {
99  for (xn::NodeInfoList::Iterator i = list.Begin(); i != list.End(); ++i) {
100  if ((*i).GetDescription().Type == XN_NODE_TYPE_DEVICE) {
101  __device = new xn::Device();
102  (*i).GetInstance(*__device);
103  break;
104  }
105  }
106  }
107  }
108 }
109 
110 
111 void
113 {
114  __openni->StopGeneratingAll();
115 
116 #if XN_VERSION_GE(1,3,2,0)
117  __openni->Release();
118 #else
119  __openni->Shutdown();
120 #endif
121  __openni.clear();
122  __openni_aspect_inifin.set_openni_context(__openni);
123 
124  if (__cfg_run_sensor_server) {
125  delete __device;
126  stop_sensor_server();
127  }
128 }
129 
130 
131 void
133 {
134  __openni.lock();
135  if (__openni.refcount() != __last_refcount) {
136  print_nodes();
137  __last_refcount = __openni.refcount();
138  }
139  __openni->WaitNoneUpdateAll();
140 
141  __check_now.stamp();
142  if ((__check_now - &__check_last) > 5) {
143  verify_active();
144  __check_last = __check_now;
145  }
146 
147  __openni.unlock();
148 }
149 
150 inline const char *
151 type_to_string(XnProductionNodeType type)
152 {
153  switch (type) {
154  case XN_NODE_TYPE_DEVICE: return "device";
155  case XN_NODE_TYPE_DEPTH: return "depth";
156  case XN_NODE_TYPE_IMAGE: return "image";
157  case XN_NODE_TYPE_AUDIO: return "audio";
158  case XN_NODE_TYPE_IR: return "IR";
159  case XN_NODE_TYPE_USER: return "user";
160  case XN_NODE_TYPE_RECORDER: return "recorder";
161  case XN_NODE_TYPE_PLAYER: return "player";
162  case XN_NODE_TYPE_GESTURE: return "gesture";
163  case XN_NODE_TYPE_SCENE: return "scene";
164  case XN_NODE_TYPE_HANDS: return "hands";
165  case XN_NODE_TYPE_CODEC: return "codec";
166  default: return "unknown";
167  }
168 }
169 
170 /** Print active nodes to log.
171  * Assumes that the context has been locked.
172  */
173 void
174 OpenNiContextThread::print_nodes()
175 {
176  xn::NodeInfoList nodes;
177  if (__openni->EnumerateExistingNodes(nodes) == XN_STATUS_OK) {
178  logger->log_info(name(), "Currently existing nodes:");
179  for (xn::NodeInfoList::Iterator n = nodes.Begin(); n != nodes.End(); ++n) {
180  const XnProductionNodeDescription &pnd = (*n).GetDescription();
181  const char *info = (*n).GetCreationInfo();
182  if (strlen(info) == 0) info = NULL;
183 
184  xn::Generator generator;
185  bool have_gen = ((*n).GetInstance(generator) == XN_STATUS_OK);
186 
187  logger->log_info(name(), " %-8s %8s (type: %-8s vendor: %-12s name: %-24s "
188  "version: %u.%u.%u.%u%s%s)",
189  (*n).GetInstanceName(),
190  have_gen ? (generator.IsGenerating() ? "active" : "inactive") : "unknown",
191  type_to_string(pnd.Type), pnd.strVendor, pnd.strName,
192  pnd.Version.nMajor, pnd.Version.nMinor, pnd.Version.nMaintenance,
193  pnd.Version.nBuild, info ? " info: " : "", info ? info : "");
194  }
195  }
196 }
197 
198 
199 /** Verify that all nodes are active.
200  * Assumes that the context has been locked.
201  */
202 void
203 OpenNiContextThread::verify_active()
204 {
205  xn::NodeInfoList nodes;
206  if (__openni->EnumerateExistingNodes(nodes) == XN_STATUS_OK) {
207  for (xn::NodeInfoList::Iterator n = nodes.Begin(); n != nodes.End(); ++n) {
208  xn::Generator generator;
209  bool have_gen = ((*n).GetInstance(generator) == XN_STATUS_OK);
210 
211  if (have_gen) {
212  const XnProductionNodeDescription &pnd = (*n).GetDescription();
213  // do not verify on device nodes for now, always reports inactive :-/
214  if (pnd.Type != XN_NODE_TYPE_DEVICE) {
215  if (! generator.IsGenerating()) {
216  logger->log_warn(name(), "Inactive node '%s' (%s, %s/%s), trying to activate",
217  (*n).GetInstanceName(), type_to_string(pnd.Type),
218  pnd.strVendor, pnd.strName);
219  generator.StartGenerating();
220 
221  } else if (! generator.IsDataNew()) {
222  if (__dead_loops.find((*n).GetInstanceName()) != __dead_loops.end()) {
223  __dead_loops[(*n).GetInstanceName()] += 1;
224  } else {
225  __dead_loops[(*n).GetInstanceName()] = 1;
226  }
227 
228  } else if (__dead_loops.find((*n).GetInstanceName()) != __dead_loops.end()) {
229  __dead_loops.erase((*n).GetInstanceName());
230  }
231 
232  /* The following does not work atm because IsDataNew() always reports false.
233  While this could be because the WaitNoneUpdateAll() did not yet update the
234  device node, event the timestamp does not change, therefore rendering this
235  way to detect death of a device node unusable.
236 
237  } else if (pnd.Type == XN_NODE_TYPE_DEVICE) {
238  // as an alternative, verify how often it has not been updated
239  if (generator.IsDataNew()) {
240  __device_no_data_loops = 0;
241  } else {
242  if (++__device_no_data_loops > 10) {
243  logger->log_warn(name(), "Device '%s' had no fresh data for long time. "
244  "Reload maybe necessary.", (*n).GetInstanceName());
245  }
246  }
247  */
248  }
249 
250  xn::ErrorStateCapability ecap = generator.GetErrorStateCap();
251  if (ecap.GetErrorState() != XN_STATUS_OK) {
252  logger->log_warn(name(), "ERROR in node '%s': %s", (*n).GetInstanceName(),
253  xnGetStatusString(ecap.GetErrorState()));
254  }
255  }
256  }
257  }
258 
259  std::map<std::string, unsigned int>::iterator d;
260  for (d = __dead_loops.begin(); d != __dead_loops.end(); ++d) {
261  if (d->second >= 3) {
262  logger->log_warn(name(), "Node '%s' had no fresh data for long time (%u tests)",
263  d->first.c_str(), d->second);
264  }
265  }
266 }
267 
268 /** Start the sensor server daemon. */
269 void
270 OpenNiContextThread::start_sensor_server()
271 {
272  if (__sensor_server_pid != -1) {
273  throw Exception("Sensor server appears to be already running");
274  }
275 
276  logger->log_info(name(), "Starting XnSensorServer");
277 
278  pid_t pid = fork();
279  if (pid == -1) {
280  throw Exception(errno, "Forking for new process failed: %s");
281  } else if (pid == 0) {
282  // child
283  setsid();
284  // ignore SIGINT, we will propagate it as SIGTERM on unload
285  signal(SIGINT, SIG_IGN);
286  fclose(stdout);
287  fclose(stdin);
288  fclose(stderr);
289  char *argv[] = {(char *)__cfg_sensor_bin.c_str(), NULL};
290  if (execve(__cfg_sensor_bin.c_str(), argv, environ) == -1) {
291  throw Exception("Failed to execute %s, exited with %i: %s\n",
292  __cfg_sensor_bin.c_str(), errno, strerror(errno));
293  }
294  }
295 
296  __sensor_server_pid = pid;
297 }
298 
299 void
300 OpenNiContextThread::stop_sensor_server()
301 {
302  if (__sensor_server_pid == -1) {
303  throw Exception("Sensor server appears not to be already running");
304  }
305 
306  logger->log_info(name(), "Stopping XnSensorServer");
307  ::kill(__sensor_server_pid, SIGTERM);
308  for (unsigned int i = 0; i < 200; ++i) {
309  usleep(10000);
310  int status;
311  int rv = waitpid(__sensor_server_pid, &status, WNOHANG);
312  if (rv == -1) {
313  if (errno == EINTR) continue;
314  if (errno == ECHILD) {
315  __sensor_server_pid = -1;
316  break;
317  }
318  } else if (rv > 0) {
319  __sensor_server_pid = -1;
320  break;
321  }
322  }
323 
324  if (__sensor_server_pid != -1) {
325  logger->log_warn(name(), "Killing XnSensorServer");
326  ::kill(__sensor_server_pid, SIGKILL);
327  __sensor_server_pid = -1;
328  }
329 }
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void unlock() const
Unlock object mutex.
Definition: lockptr.h:255
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void finalize()
Finalize the thread.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void set_clock(Clock *clock)
Set clock for this instance.
Definition: time.cpp:329
void set_openni_context(LockPtr< xn::Context > openni_context)
Set the OpenNI context to use for aspect initialization.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Thread aspect provide a new aspect.
virtual void init()
Initialize the thread.
void kill(int sig)
Send signal to a thread.
Definition: thread.cpp:668
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: lockptr.h:492
virtual void loop()
Code to execute in the thread.
OpenNiContextThread()
Constructor.
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
virtual ~OpenNiContextThread()
Destructor.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:247
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
int refcount() const
Get current refcount.
Definition: lockptr.h:229