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
34using 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
57void
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
108void
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
127void
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
146inline const char *
147type_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 */
169void
170OpenNiContextThread::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
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 */
205void
206OpenNiContextThread::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()) {
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) {
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) {
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. */
279void
280OpenNiContextThread::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
311void
312OpenNiContextThread::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}
OpenNiContextThread()
Constructor.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
virtual ~OpenNiContextThread()
Destructor.
virtual void finalize()
Finalize the thread.
Thread aspect provide a new aspect.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:257
int refcount() const
Get current refcount.
Definition: lockptr.h:228
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: lockptr.h:499
void unlock() const
Unlock object mutex.
Definition: lockptr.h:273
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
void set_openni_context(LockPtr< xn::Context > openni_context)
Set the OpenNI context to use for aspect initialization.
Thread class encapsulation of pthreads.
Definition: thread.h:46
void kill(int sig)
Send signal to a thread.
Definition: thread.cpp:662
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_clock(Clock *clock)
Set clock for this instance.
Definition: time.cpp:308
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
Fawkes library namespace.