Merge pull request #29 from christiankerl/fix_device_enumeration

enhance device enumeration
 - implements the methods to get device serial numbers and to open a device identified by its serial number
 - resets device inside openDevice method

Author: christiankerl

examples/protonect/Protonect.cpp

@@ -91,7 +91,7 @@ int main(int argc, char *argv[])
     cv::imshow("depth", cv::Mat(depth->height, depth->width, CV_32FC1, depth->data) / 4500.0f);
 
     int key = cv::waitKey(1);
-    shutdown = key > 0 && ((key & 0xFF) == 27); // shutdown on escape
+    shutdown = shutdown || (key > 0 && ((key & 0xFF) == 27)); // shutdown on escape
 
     listener.release(frames);
     //libfreenect2::this_thread::sleep_for(libfreenect2::chrono::milliseconds(100));

examples/protonect/src/libfreenect2.cpp

@@ -82,7 +82,7 @@ class Freenect2DeviceImpl : public Freenect2Device
   Freenect2Device::IrCameraParams ir_camera_params_;
   Freenect2Device::ColorCameraParams rgb_camera_params_;
 public:
-  Freenect2DeviceImpl(Freenect2Impl *context, libusb_device *usb_device, libusb_device_handle *usb_device_handle);
+  Freenect2DeviceImpl(Freenect2Impl *context, libusb_device *usb_device, libusb_device_handle *usb_device_handle, const std::string &serial);
   virtual ~Freenect2DeviceImpl();
 
   bool isSameUsbDevice(libusb_device* other);
@@ -104,14 +104,32 @@ class Freenect2DeviceImpl : public Freenect2Device
   virtual void close();
 };
 
+struct PrintBusAndDevice
+{
+  libusb_device *dev_;
+
+  PrintBusAndDevice(libusb_device *dev) : dev_(dev) {}
+};
+
+std::ostream &operator<<(std::ostream &out, const PrintBusAndDevice& dev)
+{
+  out << "@" << int(libusb_get_bus_number(dev.dev_)) << ":" << int(libusb_get_port_number(dev.dev_));
+  return out;
+}
+
 class Freenect2Impl
 {
 private:
   bool managed_usb_context_;
   libusb_context *usb_context_;
   EventLoop usb_event_loop_;
 public:
-  typedef std::vector<libusb_device *> UsbDeviceVector;
+  struct UsbDeviceWithSerial
+  {
+    libusb_device *dev;
+    std::string serial;
+  };
+  typedef std::vector<UsbDeviceWithSerial> UsbDeviceVector;
   typedef std::vector<Freenect2DeviceImpl *> DeviceVector;
 
   bool has_device_enumeration_;
@@ -203,7 +221,7 @@ class Freenect2Impl
     // free enumerated device pointers, this should not affect opened devices
     for(UsbDeviceVector::iterator it = enumerated_devices_.begin(); it != enumerated_devices_.end(); ++it)
     {
-      libusb_unref_device(*it);
+      libusb_unref_device(it->dev);
     }
 
     enumerated_devices_.clear();
@@ -225,18 +243,57 @@ class Freenect2Impl
         libusb_device *dev = device_list[idx];
         libusb_device_descriptor dev_desc;
 
-        int r = libusb_get_device_descriptor(dev, &dev_desc);
-        // TODO: error handling
+        int r = libusb_get_device_descriptor(dev, &dev_desc); // this is always successful
 
         if(dev_desc.idVendor == Freenect2Device::VendorId && (dev_desc.idProduct == Freenect2Device::ProductId || dev_desc.idProduct == Freenect2Device::ProductIdPreview))
         {
-          // valid Kinect v2
-          enumerated_devices_.push_back(dev);
-        }
-        else
-        {
-          libusb_unref_device(dev);
+          Freenect2DeviceImpl *freenect2_dev;
+
+          // prevent error if device is already open
+          if(tryGetDevice(dev, &freenect2_dev))
+          {
+            UsbDeviceWithSerial dev_with_serial;
+            dev_with_serial.dev = dev;
+            dev_with_serial.serial = freenect2_dev->getSerialNumber();
+
+            enumerated_devices_.push_back(dev_with_serial);
+            continue;
+          }
+          else
+          {
+            libusb_device_handle *dev_handle;
+            r = libusb_open(dev, &dev_handle);
+
+            if(r == LIBUSB_SUCCESS)
+            {
+              unsigned char buffer[1024];
+              r = libusb_get_string_descriptor_ascii(dev_handle, dev_desc.iSerialNumber, buffer, sizeof(buffer));
+
+              if(r > LIBUSB_SUCCESS)
+              {
+                UsbDeviceWithSerial dev_with_serial;
+                dev_with_serial.dev = dev;
+                dev_with_serial.serial = std::string(reinterpret_cast<char *>(buffer), size_t(r));
+
+                std::cout << "[Freenect2Impl] found valid Kinect v2 " << PrintBusAndDevice(dev) << " with serial " << dev_with_serial.serial << std::endl;
+                // valid Kinect v2
+                enumerated_devices_.push_back(dev_with_serial);
+                continue;
+              }
+              else
+              {
+                std::cout << "[Freenect2Impl] failed to get serial number of Kinect v2 " << PrintBusAndDevice(dev) << "!" << std::endl;
+              }
+
+              libusb_close(dev_handle);
+            }
+            else
+            {
+              std::cout << "[Freenect2Impl] failed to open Kinect v2 " << PrintBusAndDevice(dev) << "!" << std::endl;
+            }
+          }
         }
+        libusb_unref_device(dev);
       }
     }
 
@@ -261,7 +318,7 @@ Freenect2Device::~Freenect2Device()
 {
 }
 
-Freenect2DeviceImpl::Freenect2DeviceImpl(Freenect2Impl *context, libusb_device *usb_device, libusb_device_handle *usb_device_handle) :
+Freenect2DeviceImpl::Freenect2DeviceImpl(Freenect2Impl *context, libusb_device *usb_device, libusb_device_handle *usb_device_handle, const std::string &serial) :
   state_(Created),
   has_usb_interfaces_(false),
   context_(context),
@@ -276,7 +333,7 @@ Freenect2DeviceImpl::Freenect2DeviceImpl(Freenect2Impl *context, libusb_device *
   depth_packet_processor_(0),
   rgb_packet_parser_(&rgb_packet_processor_),
   depth_packet_parser_(&depth_packet_processor_),
-  serial_("<unknown>"),
+  serial_(serial),
   firmware_("<unknown>")
 {
   rgb_transfer_pool_.setCallback(&rgb_packet_parser_);
@@ -400,7 +457,12 @@ void Freenect2DeviceImpl::start()
   std::cout << GenericResponse(result.data, result.length).toString() << std::endl;
 
   command_tx_.execute(ReadSerialNumberCommand(nextCommandSeq()), serial_result);
-  serial_ = SerialNumberResponse(serial_result.data, serial_result.length).toString();
+  std::string new_serial = SerialNumberResponse(serial_result.data, serial_result.length).toString();
+
+  if(serial_ != new_serial)
+  {
+    std::cout << "[Freenect2DeviceImpl] serial number reported by libusb " << serial_ << " differs from serial number " << new_serial << " in device protocol! " << std::endl;
+  }
 
   command_tx_.execute(ReadDepthCameraParametersCommand(nextCommandSeq()), result);
   DepthCameraParamsResponse *ir_p = reinterpret_cast<DepthCameraParamsResponse *>(result.data);
@@ -561,7 +623,7 @@ int Freenect2::enumerateDevices()
 
 std::string Freenect2::getDeviceSerialNumber(int idx)
 {
-  throw std::exception();
+  return impl_->enumerated_devices_[idx].serial;
 }
 
 std::string Freenect2::getDefaultDeviceSerialNumber()
@@ -572,51 +634,68 @@ std::string Freenect2::getDefaultDeviceSerialNumber()
 Freenect2Device *Freenect2::openDevice(int idx)
 {
   int num_devices = impl_->getNumDevices();
+  Freenect2DeviceImpl *device = 0;
 
   if(idx < num_devices)
   {
-    libusb_device *dev = impl_->enumerated_devices_[idx];
+    Freenect2Impl::UsbDeviceWithSerial &dev = impl_->enumerated_devices_[idx];
     libusb_device_handle *dev_handle;
 
-    Freenect2DeviceImpl *device;
-
-    if(impl_->tryGetDevice(dev, &device))
-    {
-      return device;
-    }
-    else
+    if(!impl_->tryGetDevice(dev.dev, &device))
     {
-      int r = libusb_open(dev, &dev_handle);
-      // TODO: error handling
-
-      device = new Freenect2DeviceImpl(impl_, dev, dev_handle);
-      impl_->addDevice(device);
+      int r = libusb_open(dev.dev, &dev_handle);
 
-      if(device->open())
+      if(r == LIBUSB_SUCCESS)
       {
-        return device;
+        r = libusb_reset_device(dev_handle);
+
+        if(r == LIBUSB_SUCCESS)
+        {
+          device = new Freenect2DeviceImpl(impl_, dev.dev, dev_handle, dev.serial);
+          impl_->addDevice(device);
+
+          if(!device->open())
+          {
+            delete device;
+            device = 0;
+
+            std::cout << "[Freenect2DeviceImpl] failed to open Kinect v2 " << PrintBusAndDevice(dev.dev) << "!" << std::endl;
+          }
+        }
+        else
+        {
+          std::cout << "[Freenect2Impl] failed to reset Kinect v2 " << PrintBusAndDevice(dev.dev) << "!" << std::endl;
+        }
       }
       else
       {
-        std::cout << "[Freenect2DeviceImpl] Unable to open device: " << idx << std::endl;
-        delete device;
-
-        // TODO: error handling
-        return 0;
+        std::cout << "[Freenect2Impl] failed to open Kinect v2 " << PrintBusAndDevice(dev.dev) << "!" << std::endl;
       }
     }
   }
   else
   {
-    std::cout << "[Freenect2DeviceImpl] Requested device " << idx << " is not connected!" << std::endl;
-    // TODO: error handling
-    return 0;
+    std::cout << "[Freenect2Impl] requested device " << idx << " is not connected!" << std::endl;
   }
+
+  return device;
 }
 
 Freenect2Device *Freenect2::openDevice(const std::string &serial)
 {
-  throw std::exception();
+  Freenect2Device *device = 0;
+  int num_devices = impl_->getNumDevices();
+
+  for(int idx = 0; idx < num_devices; ++idx)
+  {
+    if(impl_->enumerated_devices_[idx].serial == serial)
+    {
+      device = openDevice(idx);
+      break;
+    }
+  }
+
+  return device;
 }
 
 Freenect2Device *Freenect2::openDefaultDevice()