Merge pull request #400 from nneonneo/openni2-crash-fix

Fix a crash-on-exit in the OpenNI2 driver

Author: piedar

OpenNI2-FreenectDriver/src/DeviceDriver.cpp

@@ -210,7 +210,20 @@ namespace FreenectDriver
   class Driver : public oni::driver::DriverBase, private Freenect::Freenect
   {
   private:
-    std::map<OniDeviceInfo, oni::driver::DeviceBase*> devices;
+    typedef std::map<OniDeviceInfo, oni::driver::DeviceBase*> OniDeviceMap;
+    OniDeviceMap devices;
+
+    static std::string devid_to_uri(int id) {
+      return "freenect://" + to_string(id);
+    }
+
+    static int uri_to_devid(const std::string uri) {
+      int id;
+      std::istringstream is(uri);
+      is.seekg(strlen("freenect://"));
+      is >> id;
+      return id;
+    }
 
   public:
     Driver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices)
@@ -230,8 +243,8 @@ namespace FreenectDriver
       DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie);
       for (int i = 0; i < Freenect::deviceCount(); i++)
       {
-        std::string uri = "freenect://" + to_string(i);
-        
+        std::string uri = devid_to_uri(i);
+
         WriteMessage("Found device " + uri);
 
         OniDeviceInfo info;
@@ -247,7 +260,7 @@ namespace FreenectDriver
 
     oni::driver::DeviceBase* deviceOpen(const char* uri, const char* mode = NULL)
     {
-      for (std::map<OniDeviceInfo, oni::driver::DeviceBase*>::iterator iter = devices.begin(); iter != devices.end(); iter++)
+      for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++)
       {
         if (strcmp(iter->first.uri, uri) == 0) // found
         {
@@ -258,11 +271,7 @@ namespace FreenectDriver
           else 
           {
             WriteMessage("Opening device " + std::string(uri));
-            
-            unsigned int id;
-            std::istringstream is(iter->first.uri);
-            is.seekg(strlen("freenect://"));
-            is >> id;
+            int id = uri_to_devid(iter->first.uri);
             Device* device = &createDevice<Device>(id);
             iter->second = device;
             return device;
@@ -276,24 +285,16 @@ namespace FreenectDriver
 
     void deviceClose(oni::driver::DeviceBase* pDevice)
     {
-      for (std::map<OniDeviceInfo, oni::driver::DeviceBase*>::iterator iter = devices.begin(); iter != devices.end();)
+      for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++)
       {
         if (iter->second == pDevice)
         {
           WriteMessage("Closing device " + std::string(iter->first.uri));
-          
-          unsigned int id;
-          std::istringstream is(std::string(iter->first.uri));
-          is.seekg(strlen("freenect://"));
-          is >> id;
-          devices.erase(iter++);
+          int id = uri_to_devid(iter->first.uri);
+          devices.erase(iter);
           deleteDevice(id);
           return;
         }
-        else
-        {
-          iter++;
-        }
       }
 
       LogError("Could not close unrecognized device");
@@ -310,8 +311,14 @@ namespace FreenectDriver
 
     void shutdown()
     {
-      for (std::map<OniDeviceInfo, oni::driver::DeviceBase*>::iterator iter = devices.begin(); iter != devices.end(); iter++)
-        deviceClose(iter->second);
+      for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++)
+      {
+        WriteMessage("Closing device " + std::string(iter->first.uri));
+        int id = uri_to_devid(iter->first.uri);
+        deleteDevice(id);
+      }
+
+      devices.clear();
     }