Merge pull request ros-drivers#18 from forouher/feature/indigo/serial

device_id: find camera by serial number
stwirth · May 22, 2014 · d2a46e8 · d2a46e8
2 parents 65777a3 + 97631ee
commit d2a46e8
Show file tree

Hide file tree

Showing 6 changed files with 50 additions and 5 deletions.
diff --git a/include/openni2_camera/openni2_convert.h b/include/openni2_camera/openni2_convert.h
@@ -43,7 +43,7 @@
 namespace openni2_wrapper
 {
 
-const OpenNI2DeviceInfo openni2_convert(const openni::DeviceInfo* pInfo);
+const OpenNI2DeviceInfo openni2_convert(const openni::DeviceInfo* pInfo, std::string serial = "");
 
 const OpenNI2VideoMode openni2_convert(const openni::VideoMode& input);
 const openni::VideoMode openni2_convert(const OpenNI2VideoMode& input);

diff --git a/include/openni2_camera/openni2_device_info.h b/include/openni2_camera/openni2_device_info.h
@@ -44,6 +44,7 @@ struct OpenNI2DeviceInfo
   std::string uri_;
   std::string vendor_;
   std::string name_;
+  std::string serial_;
   uint16_t vendor_id_;
   uint16_t product_id_;
 };

diff --git a/src/openni2_convert.cpp b/src/openni2_convert.cpp
@@ -39,7 +39,7 @@
 namespace openni2_wrapper
 {
 
-const OpenNI2DeviceInfo openni2_convert(const openni::DeviceInfo* pInfo)
+const OpenNI2DeviceInfo openni2_convert(const openni::DeviceInfo* pInfo, const std::string serial)
 {
   if (!pInfo)
     THROW_OPENNI_EXCEPTION("openni2_convert called with zero pointer\n");
@@ -51,6 +51,7 @@ const OpenNI2DeviceInfo openni2_convert(const openni::DeviceInfo* pInfo)
   output.vendor_     = pInfo->getVendor();
   output.product_id_ = pInfo->getUsbProductId();
   output.vendor_id_  = pInfo->getUsbVendorId();
+  output.serial_     = serial;
 
   return output;
 }

diff --git a/src/openni2_device_info.cpp b/src/openni2_device_info.cpp
@@ -40,6 +40,7 @@ std::ostream& operator << (std::ostream& stream, const OpenNI2DeviceInfo& device
                                            ", Name: " << device_info.name_ <<
                                            ", Vendor ID: " << device_info.vendor_id_ <<
                                            ", Product ID: " << device_info.product_id_ <<
+                                           ", Serial number: " << device_info.serial_ <<
                                              ")" << std::endl;
   return stream;
 }

diff --git a/src/openni2_device_manager.cpp b/src/openni2_device_manager.cpp
@@ -110,15 +110,46 @@ class OpenNI2DeviceListener : public openni::OpenNI::DeviceConnectedListener,
   {
     boost::mutex::scoped_lock l(device_mutex_);
 
-    ROS_INFO("Device \"%s\" connected\n", pInfo->getUri());
+    const std::string serial = getSerial(pInfo->getUri());
+    const OpenNI2DeviceInfo device_info_wrapped = openni2_convert(pInfo, serial);
 
-    const OpenNI2DeviceInfo device_info_wrapped = openni2_convert(pInfo);
+    ROS_INFO("Device \"%s\" with serial number \"%s\" connected\n", pInfo->getUri(), serial.c_str());
 
     // make sure it does not exist in set before inserting
     device_set_.erase(device_info_wrapped);
     device_set_.insert(device_info_wrapped);
   }
 
+  virtual std::string getSerial(const std::string Uri)
+  {
+    openni::Device openni_device;
+    std::string ret;
+
+    // we need to open the device to query the serial number
+    if (Uri.length() > 0 && openni_device.open(Uri.c_str()) == openni::STATUS_OK)
+    {
+      int serial_len = 100;
+      char serial[serial_len];
+
+      openni::Status rc = openni_device.getProperty(openni::DEVICE_PROPERTY_SERIAL_NUMBER, serial, &serial_len);
+      if (rc == openni::STATUS_OK)
+        ret = serial;
+      else
+      {
+        ROS_ERROR("Device \"%s\": failed to query serial number.\n", Uri.c_str());
+      }
+
+      // close the device again
+      openni_device.close();
+    }
+    else
+    {
+      ROS_DEBUG("Device \"%s\": failed to open for serial number query. It's probably used by another process.\n", Uri.c_str());
+    }
+
+    return ret;
+  }
+
   virtual void onDeviceDisconnected(const openni::DeviceInfo* pInfo)
   {
     boost::mutex::scoped_lock l(device_mutex_);

diff --git a/src/openni2_driver.cpp b/src/openni2_driver.cpp
@@ -671,9 +671,20 @@ std::string OpenNI2Driver::resolveDeviceURI(const std::string& device_id) throw(
 
     THROW_OPENNI_EXCEPTION("Device not found %s", device_id_.c_str());
   }
-  // everything else is treated as device_URI directly
   else
   {
+    // check if the device id given matches a serial number of a connected device
+    boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > dev_infos =
+      device_manager_->getConnectedDeviceInfos();
+
+    for(std::vector<OpenNI2DeviceInfo>::iterator it = dev_infos->begin();
+        it != dev_infos->end(); ++it)
+    {
+      if (device_id.size()>0 && device_id == it->serial_)
+        return it->uri_;
+    }
+
+    // everything else is treated as device_URI directly
     return device_id_;
   }
 }