Please note that active projects have migrated to https://github.com/fawkesrobotics.

openni: run and maintain XnSensorServer
authorTim Niemueller <niemueller@kbsg.rwth-aachen.de>
Tue, 5 Apr 2011 12:05:19 +0000 (14:05 +0200)
committerTim Niemueller <niemueller@kbsg.rwth-aachen.de>
Tue, 5 Apr 2011 12:48:05 +0000 (14:48 +0200)
The PrimeSense sensor module starts a separate process to interact
with the camera, but the process is not closed. Rather it waits for a 10
second timeout of no client connections and then closes. Since the fork
and execution is poorly implemented, remnants like the Fawkes network
port stay active in the XnSensorServer and cause consecutive restarts of
Fawkes to fail. The workaround is to manually "killall XnSensorServer"
after stopping Fawkes or to wait 10 seconds for the timeout.

This little patch enables the context thread to run the sensor server by
itself, allowing for a proper and immediate shutdown on unloading the
openni plugin (or closing Fawkes).

cfg/default.sql
src/plugins/openni/context_thread.cpp
src/plugins/openni/context_thread.h

index a094f5b..002d8e2 100644 (file)
@@ -164,5 +164,7 @@ INSERT INTO "config" VALUES('/plugins/refboxcomm/RemoteBB/port','unsigned int',1
 INSERT INTO "config" VALUES('/plugins/refboxcomm/RemoteBB/interface_id','string','RefBoxComm','Interface of the GameStateInterface on the remote blackboard');
 INSERT INTO "config" VALUES('/plugins/openni/resolution','string','VGA','Resolution, one of QQVGA, CGA, VGA, XVGA, XGA, 720P, SXGA, UXGA, 1080P');
 INSERT INTO "config" VALUES('/plugins/openni/fps','unsigned int',30,'Desired frames per second');
+INSERT INTO "config" VALUES('/plugins/openni/run_sensor_server','bool',1,'Should we spawn the sensor server and maintain it? This solves the problem that the XnSensorServer process is not stopped by OpenNI.');
+INSERT INTO "config" VALUES('/plugins/openni/sensor_server_bin','string','/usr/bin/XnSensorServer','Full path to the XnSensorServer binary; path');
 INSERT INTO "config" VALUES('/plugins/openni-image/debayering','string','bilinear','De-bayering mode, can be bilinear or nearest_neighbor');
 COMMIT;
index a23d251..e956829 100644 (file)
 
 #include "context_thread.h"
 
+#include <cerrno>
+#include <csignal>
+#include <unistd.h>
+#include <sys/wait.h>
 #include <XnCppWrapper.h>
 
 using namespace fawkes;
@@ -40,7 +44,6 @@ OpenNiContextThread::OpenNiContextThread()
     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR),
     AspectProviderAspect("OpenNiAspect", &__openni_aspect_inifin)
 {
-  set_prepfin_conc_loop(true);
 }
 
 
@@ -53,6 +56,16 @@ OpenNiContextThread::~OpenNiContextThread()
 void
 OpenNiContextThread::init()
 {
+  __sensor_server_pid = -1;
+  __cfg_run_sensor_server = false;
+  try {
+    __cfg_run_sensor_server =
+      config->get_bool("/plugins/openni/run_sensor_server");
+  } catch (Exception &e) {} // ignore and use default
+  if (__cfg_run_sensor_server) {
+    __cfg_sensor_bin = config->get_string("/plugins/openni/sensor_server_bin");
+  }
+
   __openni = new xn::Context();
 
   XnStatus st;
@@ -68,8 +81,29 @@ OpenNiContextThread::init()
   __check_last.stamp();
 
   __device_no_data_loops = 0;
-
   __openni_aspect_inifin.set_openni_context(__openni);
+
+  if (__cfg_run_sensor_server) {
+    start_sensor_server();
+
+    // We don't want the server to die, we kill it by ourself.
+    // Therefore we hold an instance all the time. Since client
+    // connections are not properly terminated on unloading openni,
+    // setting the timeout to 0 to stop the server immediately after
+    // it is no longer used does not work.
+    xn::NodeInfoList list;
+    if (__openni->EnumerateProductionTrees(XN_NODE_TYPE_DEVICE, NULL, list)
+       == XN_STATUS_OK)
+    {
+      for (xn::NodeInfoList::Iterator i = list.Begin(); i != list.End(); ++i) {
+       if ((*i).GetDescription().Type == XN_NODE_TYPE_DEVICE) {
+         __device = new xn::Device();
+         (*i).GetInstance(*__device);
+         break;
+       }
+      }
+    }
+  }
 }
 
 
@@ -77,9 +111,15 @@ void
 OpenNiContextThread::finalize()
 {
   __openni->StopGeneratingAll();
+
   __openni->Shutdown();
   __openni.clear();
   __openni_aspect_inifin.set_openni_context(__openni);
+
+  if (__cfg_run_sensor_server) {
+    delete __device;
+    stop_sensor_server();
+  }
 }
 
 
@@ -192,3 +232,66 @@ OpenNiContextThread::verify_active()
     }
   }
 }
+
+/** Start the sensor server daemon. */
+void
+OpenNiContextThread::start_sensor_server()
+{
+  if (__sensor_server_pid != -1) {
+    throw Exception("Sensor server appears to be already running");
+  }
+
+  logger->log_info(name(), "Starting XnSensorServer");
+
+  pid_t pid = fork();
+  if (pid == -1) {
+    throw Exception(errno, "Forking for new process failed: %s");
+  } else if (pid == 0) {
+    // child
+    setsid();
+    // ignore SIGINT, we will propagate it as SIGTERM on unload
+    signal(SIGINT, SIG_IGN);
+    fclose(stdout);
+    fclose(stdin);
+    fclose(stderr);
+    char *argv[] = {(char *)__cfg_sensor_bin.c_str(), NULL};
+    if (execve(__cfg_sensor_bin.c_str(), argv, environ) == -1) {
+      throw Exception("Failed to execute %s, exited with %i: %s\n",
+                     __cfg_sensor_bin.c_str(), errno, strerror(errno));
+    }
+  }
+
+  __sensor_server_pid = pid;
+}
+
+void
+OpenNiContextThread::stop_sensor_server()
+{
+  if (__sensor_server_pid == -1) {
+    throw Exception("Sensor server appears not to be already running");
+  }
+
+  logger->log_info(name(), "Stopping XnSensorServer");
+  ::kill(__sensor_server_pid, SIGTERM);
+  for (unsigned int i = 0; i < 200; ++i) {
+    usleep(10000);
+    int status;
+    int rv = waitpid(__sensor_server_pid, &status, WNOHANG);
+    if (rv == -1) {
+      if (errno == EINTR)  continue;
+      if (errno == ECHILD) {
+       __sensor_server_pid = -1;
+       break;
+      }
+    } else if (rv > 0) {
+      __sensor_server_pid = -1;
+      break;
+    }
+  }
+
+  if (__sensor_server_pid != -1) {
+    logger->log_warn(name(), "Killing XnSensorServer");
+    ::kill(__sensor_server_pid, SIGKILL);
+    __sensor_server_pid = -1;
+  }
+}
index 0d6ecb9..b0241f0 100644 (file)
 #include <plugins/openni/aspect/openni_inifin.h>
 #include <utils/time/time.h>
 
+#include <sys/types.h>
 
 namespace xn {
   class Context;
+  class Device;
 }
 
 class OpenNiContextThread
@@ -57,6 +59,8 @@ class OpenNiContextThread
  private:
   void print_nodes();
   void verify_active();
+  void start_sensor_server();
+  void stop_sensor_server();
 
  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
  protected: virtual void run() { Thread::run(); }
@@ -65,13 +69,17 @@ class OpenNiContextThread
   fawkes::LockPtr<xn::Context>  __openni;
   fawkes::OpenNiAspectIniFin    __openni_aspect_inifin;
 
+  bool         __cfg_run_sensor_server;
+  std::string  __cfg_sensor_bin;
+  pid_t        __sensor_server_pid;
+  xn::Device  *__device;
+
   int __last_refcount;
 
   fawkes::Time __check_last;
   fawkes::Time __check_now;
 
   unsigned int __device_no_data_loops;
-
 };
 
 #endif