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 
25 #include "utils/version.h"
26 
27 #include <sys/wait.h>
28 
29 #include <XnCppWrapper.h>
30 #include <cerrno>
31 #include <csignal>
32 #include <unistd.h>
33 
34 using namespace fawkes;
35 
36 /** @class OpenNiContextThread "context_thread.h"
37  * OpenNI Context Thread.
38  * This thread maintains an OpenNI context which can be used by other
39  * threads and is provided via the OpenNiAspect.
40  *
41  * @author Tim Niemueller
42  */
43 
44 /** Constructor. */
46 : Thread("OpenNiContextThread", Thread::OPMODE_WAITFORWAKEUP),
47  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
48  AspectProviderAspect(&openni_aspect_inifin_)
49 {
50 }
51 
52 /** Destructor. */
54 {
55 }
56 
57 void
59 {
60  sensor_server_pid_ = -1;
61  cfg_run_sensor_server_ = false;
62  try {
63  cfg_run_sensor_server_ = config->get_bool("/plugins/openni/run_sensor_server");
64  } catch (Exception &e) {
65  } // 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) == XN_STATUS_OK) {
97  for (xn::NodeInfoList::Iterator i = list.Begin(); i != list.End(); ++i) {
98  if ((*i).GetDescription().Type == XN_NODE_TYPE_DEVICE) {
99  device_ = new xn::Device();
100  (*i).GetInstance(*device_);
101  break;
102  }
103  }
104  }
105  }
106 }
107 
108 void
110 {
111  openni_->StopGeneratingAll();
112 
113 #if XN_VERSION_GE(1, 3, 2, 0)
114  openni_->Release();
115 #else
116  openni_->Shutdown();
117 #endif
118  openni_.clear();
119  openni_aspect_inifin_.set_openni_context(openni_);
120 
121  if (cfg_run_sensor_server_) {
122  delete device_;
123  stop_sensor_server();
124  }
125 }
126 
127 void
129 {
130  openni_.lock();
131  if (openni_.refcount() != last_refcount_) {
132  print_nodes();
133  last_refcount_ = openni_.refcount();
134  }
135  openni_->WaitNoneUpdateAll();
136 
137  check_now_.stamp();
138  if ((check_now_ - &check_last_) > 5) {
139  verify_active();
140  check_last_ = check_now_;
141  }
142 
143  openni_.unlock();
144 }
145 
146 inline const char *
147 type_to_string(XnProductionNodeType type)
148 {
149  switch (type) {
150  case XN_NODE_TYPE_DEVICE: return "device";
151  case XN_NODE_TYPE_DEPTH: return "depth";
152  case XN_NODE_TYPE_IMAGE: return "image";
153  case XN_NODE_TYPE_AUDIO: return "audio";
154  case XN_NODE_TYPE_IR: return "IR";
155  case XN_NODE_TYPE_USER: return "user";
156  case XN_NODE_TYPE_RECORDER: return "recorder";
157  case XN_NODE_TYPE_PLAYER: return "player";
158  case XN_NODE_TYPE_GESTURE: return "gesture";
159  case XN_NODE_TYPE_SCENE: return "scene";
160  case XN_NODE_TYPE_HANDS: return "hands";
161  case XN_NODE_TYPE_CODEC: return "codec";
162  default: return "unknown";
163  }
164 }
165 
166 /** Print active nodes to log.
167  * Assumes that the context has been locked.
168  */
169 void
170 OpenNiContextThread::print_nodes()
171 {
172  xn::NodeInfoList nodes;
173  if (openni_->EnumerateExistingNodes(nodes) == XN_STATUS_OK) {
174  logger->log_info(name(), "Currently existing nodes:");
175  for (xn::NodeInfoList::Iterator n = nodes.Begin(); n != nodes.End(); ++n) {
176  const XnProductionNodeDescription &pnd = (*n).GetDescription();
177  const char * info = (*n).GetCreationInfo();
178  if (strlen(info) == 0)
179  info = NULL;
180 
181  xn::Generator generator;
182  bool have_gen = ((*n).GetInstance(generator) == XN_STATUS_OK);
183 
184  logger->log_info(name(),
185  " %-8s %8s (type: %-8s vendor: %-12s name: %-24s "
186  "version: %u.%u.%u.%u%s%s)",
187  (*n).GetInstanceName(),
188  have_gen ? (generator.IsGenerating() ? "active" : "inactive") : "unknown",
189  type_to_string(pnd.Type),
190  pnd.strVendor,
191  pnd.strName,
192  pnd.Version.nMajor,
193  pnd.Version.nMinor,
194  pnd.Version.nMaintenance,
195  pnd.Version.nBuild,
196  info ? " info: " : "",
197  info ? info : "");
198  }
199  }
200 }
201 
202 /** Verify that all nodes are active.
203  * Assumes that the context has been locked.
204  */
205 void
206 OpenNiContextThread::verify_active()
207 {
208  xn::NodeInfoList nodes;
209  if (openni_->EnumerateExistingNodes(nodes) == XN_STATUS_OK) {
210  for (xn::NodeInfoList::Iterator n = nodes.Begin(); n != nodes.End(); ++n) {
211  xn::Generator generator;
212  bool have_gen = ((*n).GetInstance(generator) == XN_STATUS_OK);
213 
214  if (have_gen) {
215  const XnProductionNodeDescription &pnd = (*n).GetDescription();
216  // do not verify on device nodes for now, always reports inactive :-/
217  if (pnd.Type != XN_NODE_TYPE_DEVICE) {
218  if (!generator.IsGenerating()) {
219  logger->log_warn(name(),
220  "Inactive node '%s' (%s, %s/%s), trying to activate",
221  (*n).GetInstanceName(),
222  type_to_string(pnd.Type),
223  pnd.strVendor,
224  pnd.strName);
225  generator.StartGenerating();
226 
227  } else if (!generator.IsDataNew()) {
228  if (dead_loops_.find((*n).GetInstanceName()) != dead_loops_.end()) {
229  dead_loops_[(*n).GetInstanceName()] += 1;
230  } else {
231  dead_loops_[(*n).GetInstanceName()] = 1;
232  }
233 
234  } else if (dead_loops_.find((*n).GetInstanceName()) != dead_loops_.end()) {
235  dead_loops_.erase((*n).GetInstanceName());
236  }
237 
238  /* The following does not work atm because IsDataNew() always reports false.
239  While this could be because the WaitNoneUpdateAll() did not yet update the
240  device node, event the timestamp does not change, therefore rendering this
241  way to detect death of a device node unusable.
242 
243  } else if (pnd.Type == XN_NODE_TYPE_DEVICE) {
244  // as an alternative, verify how often it has not been updated
245  if (generator.IsDataNew()) {
246  device_no_data_loops_ = 0;
247  } else {
248  if (++device_no_data_loops_ > 10) {
249  logger->log_warn(name(), "Device '%s' had no fresh data for long time. "
250  "Reload maybe necessary.", (*n).GetInstanceName());
251  }
252  }
253  */
254  }
255 
256  xn::ErrorStateCapability ecap = generator.GetErrorStateCap();
257  if (ecap.GetErrorState() != XN_STATUS_OK) {
258  logger->log_warn(name(),
259  "ERROR in node '%s': %s",
260  (*n).GetInstanceName(),
261  xnGetStatusString(ecap.GetErrorState()));
262  }
263  }
264  }
265  }
266 
267  std::map<std::string, unsigned int>::iterator d;
268  for (d = dead_loops_.begin(); d != dead_loops_.end(); ++d) {
269  if (d->second >= 3) {
270  logger->log_warn(name(),
271  "Node '%s' had no fresh data for long time (%u tests)",
272  d->first.c_str(),
273  d->second);
274  }
275  }
276 }
277 
278 /** Start the sensor server daemon. */
279 void
280 OpenNiContextThread::start_sensor_server()
281 {
282  if (sensor_server_pid_ != -1) {
283  throw Exception("Sensor server appears to be already running");
284  }
285 
286  logger->log_info(name(), "Starting XnSensorServer");
287 
288  pid_t pid = fork();
289  if (pid == -1) {
290  throw Exception(errno, "Forking for new process failed: %s");
291  } else if (pid == 0) {
292  // child
293  setsid();
294  // ignore SIGINT, we will propagate it as SIGTERM on unload
295  signal(SIGINT, SIG_IGN);
296  fclose(stdout);
297  fclose(stdin);
298  fclose(stderr);
299  char *argv[] = {(char *)cfg_sensor_bin_.c_str(), NULL};
300  if (execve(cfg_sensor_bin_.c_str(), argv, environ) == -1) {
301  throw Exception("Failed to execute %s, exited with %i: %s\n",
302  cfg_sensor_bin_.c_str(),
303  errno,
304  strerror(errno));
305  }
306  }
307 
308  sensor_server_pid_ = pid;
309 }
310 
311 void
312 OpenNiContextThread::stop_sensor_server()
313 {
314  if (sensor_server_pid_ == -1) {
315  throw Exception("Sensor server appears not to be already running");
316  }
317 
318  logger->log_info(name(), "Stopping XnSensorServer");
319  ::kill(sensor_server_pid_, SIGTERM);
320  for (unsigned int i = 0; i < 200; ++i) {
321  usleep(10000);
322  int status;
323  int rv = waitpid(sensor_server_pid_, &status, WNOHANG);
324  if (rv == -1) {
325  if (errno == EINTR)
326  continue;
327  if (errno == ECHILD) {
328  sensor_server_pid_ = -1;
329  break;
330  }
331  } else if (rv > 0) {
332  sensor_server_pid_ = -1;
333  break;
334  }
335  }
336 
337  if (sensor_server_pid_ != -1) {
338  logger->log_warn(name(), "Killing XnSensorServer");
339  ::kill(sensor_server_pid_, SIGKILL);
340  sensor_server_pid_ = -1;
341  }
342 }
fawkes::Thread::kill
void kill(int sig)
Send signal to a thread.
Definition: thread.cpp:662
OpenNiContextThread::finalize
virtual void finalize()
Finalize the thread.
Definition: context_thread.cpp:109
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
fawkes::LockPtr::refcount
int refcount() const
Get current refcount.
Definition: lockptr.h:228
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
fawkes::AspectProviderAspect
Thread aspect provide a new aspect.
Definition: aspect_provider.h:36
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::ClockAspect::clock
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
OpenNiContextThread::init
virtual void init()
Initialize the thread.
Definition: context_thread.cpp:58
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::LockPtr::unlock
void unlock() const
Unlock object mutex.
Definition: lockptr.h:273
fawkes
Fawkes library namespace.
fawkes::LockPtr::clear
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: lockptr.h:499
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
fawkes::Time::set_clock
void set_clock(Clock *clock)
Set clock for this instance.
Definition: time.cpp:308
OpenNiContextThread::loop
virtual void loop()
Code to execute in the thread.
Definition: context_thread.cpp:128
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
OpenNiContextThread::OpenNiContextThread
OpenNiContextThread()
Constructor.
Definition: context_thread.cpp:45
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
OpenNiContextThread::~OpenNiContextThread
virtual ~OpenNiContextThread()
Destructor.
Definition: context_thread.cpp:53
fawkes::LockPtr::lock
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:257
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::Time::stamp
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
fawkes::OpenNiAspectIniFin::set_openni_context
void set_openni_context(LockPtr< xn::Context > openni_context)
Set the OpenNI context to use for aspect initialization.
Definition: openni_inifin.cpp:70
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36