openni_driver.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011 2011 Willow Garage, Inc.
00005  *    Suat Gedikli <gedikli@willowgarage.com>
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  */
00037 #include <openni_camera/openni_driver.h>
00038 #include <openni_camera/openni_device_kinect.h>
00039 #include <openni_camera/openni_device_primesense.h>
00040 #include <openni_camera/openni_device_xtion.h>
00041 #include <openni_camera/openni_device_oni.h>
00042 #include <sstream>
00043 #include <iostream>
00044 #include <algorithm>
00045 #include <locale>
00046 #include <cctype>
00047 #include <map>
00048 
00049 #ifndef _WIN32
00050 #include <libusb-1.0/libusb.h>
00051 #else
00052 #include <boost/tokenizer.hpp>
00053 #endif
00054 
00055 using namespace std;
00056 using namespace boost;
00057 
00058 namespace openni_wrapper
00059 {
00060 
00061 OpenNIDriver::OpenNIDriver () throw (OpenNIException)
00062 {
00063   // Initialize the Engine
00064   XnStatus status = context_.Init ();
00065   if (status != XN_STATUS_OK)
00066     THROW_OPENNI_EXCEPTION ("initialization failed. Reason: %s", xnGetStatusString (status));
00067 
00068   updateDeviceList ();
00069 }
00070 
00071 unsigned OpenNIDriver::updateDeviceList () throw ()
00072 {
00073   // clear current list of devices
00074   device_context_.clear ();
00075   // clear maps
00076   bus_map_.clear ();
00077   serial_map_.clear ();
00078   connection_string_map_.clear ();
00079 
00080   // enumerate all devices
00081   static xn::NodeInfoList node_info_list;
00082   XnStatus status = context_.EnumerateProductionTrees (XN_NODE_TYPE_DEVICE, NULL, node_info_list);
00083   if (status != XN_STATUS_OK && node_info_list.Begin () != node_info_list.End ())
00084     THROW_OPENNI_EXCEPTION ("enumerating devices failed. Reason: %s", xnGetStatusString (status));
00085   else if (node_info_list.Begin () == node_info_list.End ())
00086     return 0; // no exception
00087 
00088   for (xn::NodeInfoList::Iterator nodeIt = node_info_list.Begin (); nodeIt != node_info_list.End (); ++nodeIt)
00089   {
00090     connection_string_map_[(*nodeIt).GetCreationInfo ()] = device_context_.size();
00091     device_context_.push_back (DeviceContext (*nodeIt));
00092   }
00093 
00094   // enumerate depth nodes
00095   static xn::NodeInfoList depth_nodes;
00096   status = context_.EnumerateProductionTrees (XN_NODE_TYPE_DEPTH, NULL, depth_nodes, NULL);
00097   if (status != XN_STATUS_OK)
00098     THROW_OPENNI_EXCEPTION ("enumerating depth generators failed. Reason: %s", xnGetStatusString (status));
00099 
00100   for (xn::NodeInfoList::Iterator nodeIt = depth_nodes.Begin (); nodeIt != depth_nodes.End (); ++nodeIt)
00101   {
00102     // check if to which device this node is assigned to
00103     for (xn::NodeInfoList::Iterator neededIt = (*nodeIt).GetNeededNodes ().Begin (); neededIt != (*nodeIt).GetNeededNodes ().End (); ++neededIt)
00104     {
00105       if ( connection_string_map_.count ((*neededIt).GetCreationInfo ()) )
00106       {
00107         unsigned device_index = connection_string_map_[(*neededIt).GetCreationInfo ()];
00108         device_context_[device_index].depth_node.reset (new xn::NodeInfo(*nodeIt));
00109       }
00110     }
00111   }
00112 
00113   // enumerate image nodes
00114   static xn::NodeInfoList image_nodes;
00115   static xn::EnumerationErrors enumeration_errors;
00116   status = context_.EnumerateProductionTrees (XN_NODE_TYPE_IMAGE, NULL, image_nodes, &enumeration_errors);
00117   if (status != XN_STATUS_OK)
00118   {
00119     // the asus pro doesn't produce images, so if the error is just "can't create node of the given type", then
00120     // we ignore it (that is what error code 65565 is). if this is some other error, for instance unable
00121     // to access the device due to permissions, then we throw an exception. -jbinney
00122     if((enumeration_errors.Begin()).Error() != 65565) {
00123       XnChar strError[1024];
00124       enumeration_errors.ToString(strError, 1024);
00125       THROW_OPENNI_EXCEPTION("enumerating image nodes failed. Reason: %s", strError);
00126     }
00127   }
00128   
00129   for (xn::NodeInfoList::Iterator nodeIt = image_nodes.Begin (); nodeIt != image_nodes.End (); ++nodeIt)
00130     {
00131       // check to which device this node is assigned to
00132       for (xn::NodeInfoList::Iterator neededIt = (*nodeIt).GetNeededNodes ().Begin (); neededIt != (*nodeIt).GetNeededNodes ().End (); ++neededIt)
00133         {
00134           if ( connection_string_map_.count ((*neededIt).GetCreationInfo ()) )
00135             {
00136               unsigned device_index = connection_string_map_[(*neededIt).GetCreationInfo ()];
00137               device_context_[device_index].image_node.reset (new xn::NodeInfo(*nodeIt));
00138             }
00139         }
00140     }
00141   
00142   // enumerate IR nodes
00143   static xn::NodeInfoList ir_nodes;
00144   status = context_.EnumerateProductionTrees (XN_NODE_TYPE_IR, NULL, ir_nodes, NULL);
00145   if (status != XN_STATUS_OK)
00146     THROW_OPENNI_EXCEPTION ("enumerating IR generators failed. Reason: %s", xnGetStatusString (status));
00147 
00148   for (xn::NodeInfoList::Iterator nodeIt = ir_nodes.Begin (); nodeIt != ir_nodes.End (); ++nodeIt)
00149   {
00150     // check if to which device this node is assigned to
00151     for (xn::NodeInfoList::Iterator neededIt = (*nodeIt).GetNeededNodes ().Begin (); neededIt != (*nodeIt).GetNeededNodes ().End (); ++neededIt)
00152     {
00153       if ( connection_string_map_.count ((*neededIt).GetCreationInfo ()) )
00154       {
00155         unsigned device_index = connection_string_map_[(*neededIt).GetCreationInfo ()];
00156         device_context_[device_index].ir_node.reset (new xn::NodeInfo(*nodeIt));
00157       }
00158     }
00159   }
00160 
00161 #ifndef _WIN32
00162   // add context object for each found device
00163   for (unsigned deviceIdx = 0; deviceIdx < device_context_.size (); ++deviceIdx)
00164   {
00165     // register bus@address to the corresponding context object
00166     unsigned short vendor_id;
00167     unsigned short product_id;
00168     unsigned char bus;
00169     unsigned char address;
00170     sscanf (device_context_[deviceIdx].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00171     bus_map_ [bus][address] = deviceIdx;
00172   }
00173 
00174   // get additional info about connected devices like serial number, vendor name and prduct name
00175   getDeviceInfos ();
00176   // build serial number -> device index map
00177   for (unsigned deviceIdx = 0; deviceIdx < device_context_.size (); ++deviceIdx)
00178   {
00179     std::string serial_number = getSerialNumber (deviceIdx);
00180     if (!serial_number.empty ())
00181       serial_map_[serial_number] = deviceIdx;
00182   }
00183 
00184 #endif
00185 
00186   // redundant, but needed for Windows right now and also for Xtion
00187   for (unsigned deviceIdx = 0; deviceIdx < device_context_.size (); ++deviceIdx)
00188   {
00189     unsigned short product_id;
00190     unsigned short vendor_id;
00191 
00192     getDeviceType(device_context_[deviceIdx].device_node.GetCreationInfo (), vendor_id, product_id );
00193 
00194 #if _WIN32
00195     if (vendor_id == 0x45e)
00196     {
00197       strcpy ((char*)device_context_[deviceIdx].device_node.GetDescription().strVendor, "Microsoft");
00198       strcpy ((char*)device_context_[deviceIdx].device_node.GetDescription().strName, "Xbox NUI Camera");
00199     }
00200     else
00201 #endif
00202     if (vendor_id == 0x1d27 && device_context_[deviceIdx].image_node.get () == 0)
00203     {
00204       strcpy ((char*)device_context_[deviceIdx].device_node.GetDescription().strVendor, "ASUS");
00205       strcpy ((char*)device_context_[deviceIdx].device_node.GetDescription().strName, "Xtion Pro");
00206     }
00207   }
00208   return (device_context_.size ());
00209 }
00210 
00211 void OpenNIDriver::stopAll () throw (OpenNIException)
00212 {
00213   XnStatus status = context_.StopGeneratingAll ();
00214   if (status != XN_STATUS_OK)
00215     THROW_OPENNI_EXCEPTION ("stopping all streams failed. Reason: %s", xnGetStatusString (status));
00216 }
00217 
00218 OpenNIDriver::~OpenNIDriver () throw ()
00219 {
00220   // no exception during destuctor
00221   try
00222   {
00223     stopAll ();
00224   }
00225   catch (...)
00226   {
00227   }
00228 
00229   context_.Shutdown ();
00230 }
00231 
00232 boost::shared_ptr<OpenNIDevice> OpenNIDriver::createVirtualDevice (const string& path, bool repeat, bool stream) const throw (OpenNIException)
00233 {
00234   return boost::shared_ptr<OpenNIDevice> (new DeviceONI (context_, path, repeat, stream));
00235 }
00236 
00237 void OpenNIDriver::getPrimesenseSerial(xn::NodeInfo info, char* buffer, unsigned buf_size) const throw () {
00238 
00239         context_.CreateProductionTree(info);
00240         xn::Device device;
00241 
00242         if(info.GetInstance(device) != XN_STATUS_OK) {
00243             THROW_OPENNI_EXCEPTION ("couldn't get device instance for reading serial no.");
00244         }
00245 
00246         xn::DeviceIdentificationCapability d = device.GetIdentificationCap();
00247 
00248         d.GetSerialNumber(buffer,buf_size);
00249 
00250         device.Release();
00251 }
00252 
00253 boost::shared_ptr<OpenNIDevice> OpenNIDriver::getDeviceByIndex (unsigned index) const throw (OpenNIException)
00254 {
00255   using namespace std;
00256 
00257   if (index >= device_context_.size ())
00258     THROW_OPENNI_EXCEPTION ("device index out of range. only %d devices connected but device %d requested.", device_context_.size (), index);
00259   boost::shared_ptr<OpenNIDevice> device = device_context_[index].device.lock ();
00260   if (!device)
00261   {
00262     unsigned short vendor_id;
00263     unsigned short product_id;
00264     getDeviceType (device_context_[index].device_node.GetCreationInfo (), vendor_id, product_id );
00265 
00266     if (vendor_id == 0x45e)
00267     {
00268       device = boost::shared_ptr<OpenNIDevice > (new DeviceKinect (context_, device_context_[index].device_node,
00269                                                                    *device_context_[index].image_node, *device_context_[index].depth_node,
00270                                                                    *device_context_[index].ir_node));
00271       device_context_[index].device = device;
00272     }
00273     else if (vendor_id == 0x1d27)
00274     {
00275       if (device_context_[index].image_node.get())
00276         device = boost::shared_ptr<OpenNIDevice > (new DevicePrimesense (context_, device_context_[index].device_node,
00277                                                                          *device_context_[index].image_node, *device_context_[index].depth_node,
00278                                                                          *device_context_[index].ir_node));
00279       else
00280         device = boost::shared_ptr<OpenNIDevice > (new DeviceXtionPro (context_, device_context_[index].device_node,
00281                                                                        *device_context_[index].depth_node, *device_context_[index].ir_node));
00282       device_context_[index].device = device;
00283     }
00284     else
00285     {
00286       THROW_OPENNI_EXCEPTION ("vendor %s (%s) known by primesense driver, but not by ros driver. Contact maintainer of the ros driver.",
00287                               getVendorName (index), vendor_id);
00288     }
00289   }
00290   return (device);
00291 }
00292 
00293 #ifndef _WIN32
00294 boost::shared_ptr<OpenNIDevice> 
00295 OpenNIDriver::getDeviceBySerialNumber (const std::string& serial_number) const throw (OpenNIException)
00296 {
00297   std::map<std::string, unsigned>::const_iterator it = serial_map_.find (serial_number);
00298 
00299   if (it != serial_map_.end ())
00300   {
00301     return getDeviceByIndex (it->second);
00302   }
00303 
00304   THROW_OPENNI_EXCEPTION ("No device with serial number \'%s\' found", serial_number.c_str ());
00305 
00306   // because of warnings!!!
00307   return (boost::shared_ptr<OpenNIDevice > ((OpenNIDevice*)NULL));
00308 }
00309 
00310 boost::shared_ptr<OpenNIDevice> 
00311 OpenNIDriver::getDeviceByAddress (unsigned char bus, unsigned char address) const throw (OpenNIException)
00312 {
00313   std::map<unsigned char, std::map<unsigned char, unsigned> >::const_iterator busIt = bus_map_.find (bus);
00314   if (busIt != bus_map_.end ())
00315   {
00316     std::map<unsigned char, unsigned>::const_iterator devIt;
00317     // Interpret invalid address 0 as "open an arbitrary device on this bus"
00318     if (address == 0)
00319       devIt = busIt->second.begin ();
00320     else
00321       devIt = busIt->second.find (address);
00322     
00323     if (devIt != busIt->second.end ())
00324     {
00325       return getDeviceByIndex (devIt->second);
00326     }
00327   }
00328 
00329   THROW_OPENNI_EXCEPTION ("No device on bus: %d @ %d found", (int)bus, (int)address);
00330 
00331   // because of warnings!!!
00332   return (boost::shared_ptr<OpenNIDevice > ((OpenNIDevice*)NULL));
00333 }
00334 
00335 void 
00336 OpenNIDriver::getDeviceInfos () throw ()
00337 {
00338   libusb_context *context = NULL;
00339   int result;
00340   result = libusb_init (&context); //initialize a library session
00341 
00342   if (result < 0)
00343     return;
00344 
00345   libusb_device **devices;
00346   int count = libusb_get_device_list (context, &devices);
00347   if (count < 0)
00348     return;
00349 
00350   for (int devIdx = 0; devIdx < count; ++devIdx)
00351   {
00352     libusb_device* device = devices[devIdx];
00353     uint8_t busId = libusb_get_bus_number (device);
00354     std::map<unsigned char, std::map<unsigned char, unsigned> >::const_iterator busIt = bus_map_.find (busId);
00355     if (busIt == bus_map_.end ())
00356       continue;
00357 
00358     uint8_t address = libusb_get_device_address (device);
00359     std::map<unsigned char, unsigned>::const_iterator addressIt = busIt->second.find (address);
00360     if (addressIt == busIt->second.end ())
00361       continue;
00362 
00363     unsigned nodeIdx = addressIt->second;
00364     xn::NodeInfo& current_node = device_context_[nodeIdx].device_node;
00365 
00366     libusb_device_descriptor descriptor;
00367     result = libusb_get_device_descriptor (devices[devIdx], &descriptor);
00368 
00369     if (result < 0)
00370     {
00371       current_node.SetInstanceName ("");
00372     }
00373     else
00374     {
00375       libusb_device_handle* dev_handle;
00376       result = libusb_open (device, &dev_handle);
00377       if (result < 0)
00378       {
00379         current_node.SetInstanceName ("");
00380       }
00381       else
00382       {
00383         unsigned char buffer[1024];
00384 
00385         int len = libusb_get_string_descriptor_ascii (dev_handle, descriptor.iSerialNumber, buffer, 1024);
00386         if (len > 4)
00387           current_node.SetInstanceName ((char*)buffer);
00388         else
00389           current_node.SetInstanceName ("");
00390 
00391         libusb_close (dev_handle);
00392       }
00393     }
00394   }
00395   libusb_free_device_list (devices, 1);
00396   libusb_exit (context);
00397 }
00398 #endif
00399 
00400 const char* OpenNIDriver::getSerialNumber (unsigned index) const throw ()
00401 {
00402 #ifndef _WIN32
00403 
00404     DeviceContext con = device_context_[index];
00405     const char* openni_serial = con.device_node.GetInstanceName ();
00406 
00407     if (strlen(openni_serial)>0 && strcmp(openni_serial, "Device1")) {
00408         return openni_serial;
00409     } else {
00410         char *primesense_serial = (char*)malloc(XN_MAX_NAME_LENGTH); // memleak
00411         getPrimesenseSerial(con.device_node, primesense_serial, XN_MAX_NAME_LENGTH);
00412 
00413         return primesense_serial;
00414     }
00415 
00416 #else
00417   return "";
00418 #endif
00419 }
00420 
00421 void OpenNIDriver::getDeviceType (const std::string& connectionString, unsigned short& vendorId, unsigned short& productId)
00422 {
00423 #if _WIN32
00424     // expected format: "\\?\usb#vid_[ID]&pid_[ID]#[SERIAL]#{GUID}"
00425     typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
00426     boost::char_separator<char> separators("#&_");
00427     tokenizer tokens(connectionString, separators);
00428 
00429     unsigned int tokenIndex = 0;
00430     for (tokenizer::iterator tok_iter = tokens.begin(); tok_iter != tokens.end(); ++tok_iter, tokenIndex++)
00431     {
00432         std::string tokenValue = *tok_iter;
00433 
00434         switch(tokenIndex) {
00435             case 2:    // the vendor ID
00436                 sscanf(tokenValue.c_str(), "%hx", &vendorId);
00437                 break;
00438             case 4: // the product ID
00439                 sscanf(tokenValue.c_str(), "%hx", &productId);
00440                 break;
00441         }
00442     }
00443 #else
00444     unsigned char bus;
00445     unsigned char address;
00446     sscanf (connectionString.c_str(), "%hx/%hx@%hhu/%hhu", &vendorId, &productId, &bus, &address);
00447 #endif
00448 }
00449 
00450 const char* OpenNIDriver::getConnectionString (unsigned index) const throw ()
00451 {
00452   return device_context_[index].device_node.GetCreationInfo ();
00453 }
00454 
00455 const char* OpenNIDriver::getVendorName (unsigned index) const throw ()
00456 {
00457   return device_context_[index].device_node.GetDescription ().strVendor;
00458 }
00459 
00460 const char* OpenNIDriver::getProductName (unsigned index) const throw ()
00461 {
00462   return device_context_[index].device_node.GetDescription ().strName;
00463 }
00464 
00465 unsigned short OpenNIDriver::getVendorID (unsigned index) const throw ()
00466 {
00467   unsigned short vendor_id;
00468   unsigned short product_id;
00469 #ifndef _WIN32
00470   unsigned char bus;
00471   unsigned char address;
00472   sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00473 
00474 #else
00475   getDeviceType (device_context_[index].device_node.GetCreationInfo (), vendor_id, product_id);
00476 #endif
00477   return vendor_id;
00478 }
00479 
00480 unsigned short OpenNIDriver::getProductID (unsigned index) const throw ()
00481 {
00482   unsigned short vendor_id;
00483   unsigned short product_id;
00484 #ifndef _WIN32
00485   unsigned char bus;
00486   unsigned char address;
00487   sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00488 
00489 #else
00490   getDeviceType (device_context_[index].device_node.GetCreationInfo (), vendor_id, product_id);
00491 #endif
00492   return product_id;
00493 }
00494 
00495 unsigned char OpenNIDriver::getBus (unsigned index) const throw ()
00496 {
00497   unsigned char bus = 0;
00498 #ifndef _WIN32
00499   unsigned short vendor_id;
00500   unsigned short product_id;
00501   unsigned char address;
00502   sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00503 #endif
00504   return bus;
00505 }
00506 
00507 unsigned char OpenNIDriver::getAddress (unsigned index) const throw ()
00508 {
00509   unsigned char address = 0;
00510 #ifndef _WIN32
00511   unsigned short vendor_id;
00512   unsigned short product_id;
00513   unsigned char bus;  
00514   sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00515 #endif
00516   return address;
00517 }
00518 
00519 OpenNIDriver::DeviceContext::DeviceContext (const xn::NodeInfo& device, xn::NodeInfo* image, xn::NodeInfo* depth, xn::NodeInfo* ir)
00520 : device_node (device)
00521 , image_node (image)
00522 , depth_node (depth)
00523 , ir_node (ir)
00524 {
00525 }
00526 
00527 OpenNIDriver::DeviceContext::DeviceContext (const xn::NodeInfo& device)
00528 : device_node (device)
00529 , image_node ((xn::NodeInfo*)0)
00530 , depth_node ((xn::NodeInfo*)0)
00531 , ir_node ((xn::NodeInfo*)0)
00532 {
00533 }
00534 
00535 OpenNIDriver::DeviceContext::DeviceContext (const DeviceContext& other)
00536 : device_node (other.device_node)
00537 , image_node (other.image_node)
00538 , depth_node (other.depth_node)
00539 , ir_node (other.ir_node)
00540 , device (other.device)
00541 {
00542 }
00543 
00544 } // namespace


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Wed Aug 26 2015 15:08:49