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 <pcl/pcl_config.h>
00038 #ifdef HAVE_OPENNI
00039 
00040 #include <pcl/io/openni_camera/openni_driver.h>
00041 #include <pcl/io/openni_camera/openni_device_kinect.h>
00042 #include <pcl/io/openni_camera/openni_device_primesense.h>
00043 #include <pcl/io/openni_camera/openni_device_xtion.h>
00044 #include <pcl/io/openni_camera/openni_device_oni.h>
00045 #include <sstream>
00046 #include <iostream>
00047 #include <algorithm>
00048 #include <locale>
00049 #include <cctype>
00050 #include <map>
00051 #include <XnVersion.h>
00052 
00053 #ifndef _WIN32
00054 #include <libusb-1.0/libusb.h>
00055 #else
00056 #include <boost/foreach.hpp>
00057 #include <boost/tokenizer.hpp>
00058 #endif
00059 
00060 using namespace std;
00061 using namespace boost;
00062 
00063 namespace openni_wrapper
00064 {
00065 
00066 OpenNIDriver::OpenNIDriver ()
00067   : device_context_ ()
00068   , context_ ()
00069   , bus_map_ ()
00070   , serial_map_ ()
00071   , connection_string_map_ ()
00072 {
00073   // Initialize the Engine
00074   XnStatus status = context_.Init ();
00075   if (status != XN_STATUS_OK)
00076     THROW_OPENNI_EXCEPTION ("initialization failed. Reason: %s", xnGetStatusString (status));
00077 
00078   updateDeviceList ();
00079 }
00080 
00081 unsigned OpenNIDriver::updateDeviceList ()
00082 {
00083   // clear current list of devices
00084   device_context_.clear ();
00085   // clear maps
00086   bus_map_.clear ();
00087   serial_map_.clear ();
00088   connection_string_map_.clear ();
00089 
00090   // enumerate all devices
00091   static xn::NodeInfoList node_info_list;
00092   XnStatus status = context_.EnumerateProductionTrees (XN_NODE_TYPE_DEVICE, NULL, node_info_list);
00093   if (status != XN_STATUS_OK && node_info_list.Begin () != node_info_list.End ())
00094     THROW_OPENNI_EXCEPTION ("enumerating devices failed. Reason: %s", xnGetStatusString (status));
00095   else if (node_info_list.Begin () == node_info_list.End ())
00096     return 0; // no exception
00097 
00098   for (xn::NodeInfoList::Iterator nodeIt = node_info_list.Begin (); nodeIt != node_info_list.End (); ++nodeIt)
00099   {
00100     connection_string_map_[(*nodeIt).GetCreationInfo ()] = static_cast<unsigned int> (device_context_.size ());
00101     device_context_.push_back (DeviceContext (*nodeIt));
00102   }
00103 
00104   // enumerate depth nodes
00105   static xn::NodeInfoList depth_nodes;
00106   status = context_.EnumerateProductionTrees (XN_NODE_TYPE_DEPTH, NULL, depth_nodes, NULL);
00107   if (status != XN_STATUS_OK)
00108     THROW_OPENNI_EXCEPTION ("enumerating depth generators failed. Reason: %s", xnGetStatusString (status));
00109 
00110   for (xn::NodeInfoList::Iterator nodeIt = depth_nodes.Begin (); nodeIt != depth_nodes.End (); ++nodeIt)
00111   {
00112     // check if to which device this node is assigned to
00113     for (xn::NodeInfoList::Iterator neededIt = (*nodeIt).GetNeededNodes ().Begin (); neededIt != (*nodeIt).GetNeededNodes ().End (); ++neededIt)
00114     {
00115       if ( connection_string_map_.count ((*neededIt).GetCreationInfo ()) )
00116       {
00117         unsigned device_index = connection_string_map_[(*neededIt).GetCreationInfo ()];
00118         device_context_[device_index].depth_node.reset (new xn::NodeInfo(*nodeIt));
00119       }
00120     }
00121   }
00122 
00123   // enumerate image nodes
00124   static xn::NodeInfoList image_nodes;
00125   status = context_.EnumerateProductionTrees (XN_NODE_TYPE_IMAGE, NULL, image_nodes, NULL);
00126 
00127 
00128   // Suat: This is an ugly ASUS Xtion workaround.
00129   if (status == XN_STATUS_OK)
00130   {
00131     //THROW_OPENNI_EXCEPTION ("enumerating image generators failed. Reason: %s", xnGetStatusString (status));
00132 
00133     for (xn::NodeInfoList::Iterator nodeIt = image_nodes.Begin (); nodeIt != image_nodes.End (); ++nodeIt)
00134     {
00135       // check to which device this node is assigned to
00136       for (xn::NodeInfoList::Iterator neededIt = (*nodeIt).GetNeededNodes ().Begin (); neededIt != (*nodeIt).GetNeededNodes ().End (); ++neededIt)
00137       {
00138         if ( connection_string_map_.count ((*neededIt).GetCreationInfo ()) )
00139         {
00140           unsigned device_index = connection_string_map_[(*neededIt).GetCreationInfo ()];
00141           device_context_[device_index].image_node.reset (new xn::NodeInfo(*nodeIt));
00142         }
00143       }
00144     }
00145   }
00146 
00147   // enumerate IR nodes
00148   static xn::NodeInfoList ir_nodes;
00149   status = context_.EnumerateProductionTrees (XN_NODE_TYPE_IR, NULL, ir_nodes, NULL);
00150   if (status != XN_STATUS_OK)
00151     THROW_OPENNI_EXCEPTION ("enumerating IR generators failed. Reason: %s", xnGetStatusString (status));
00152 
00153   for (xn::NodeInfoList::Iterator nodeIt = ir_nodes.Begin (); nodeIt != ir_nodes.End (); ++nodeIt)
00154   {
00155     // check if to which device this node is assigned to
00156     for (xn::NodeInfoList::Iterator neededIt = (*nodeIt).GetNeededNodes ().Begin (); neededIt != (*nodeIt).GetNeededNodes ().End (); ++neededIt)
00157     {
00158       if ( connection_string_map_.count ((*neededIt).GetCreationInfo ()) )
00159       {
00160         unsigned device_index = connection_string_map_[(*neededIt).GetCreationInfo ()];
00161         device_context_[device_index].ir_node.reset (new xn::NodeInfo(*nodeIt));
00162       }
00163     }
00164   }
00165 
00166 #ifndef _WIN32
00167   // add context object for each found device
00168   for (unsigned deviceIdx = 0; deviceIdx < device_context_.size (); ++deviceIdx)
00169   {
00170     // register bus@address to the corresponding context object
00171     unsigned short vendor_id;
00172     unsigned short product_id;
00173     unsigned char bus;
00174     unsigned char address;
00175     sscanf (device_context_[deviceIdx].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00176     bus_map_ [bus][address] = deviceIdx;
00177   }
00178 
00179   // get additional info about connected devices like serial number, vendor name and prduct name
00180   getDeviceInfos ();
00181 #endif
00182   // build serial number -> device index map
00183   for (unsigned deviceIdx = 0; deviceIdx < device_context_.size (); ++deviceIdx)
00184   {
00185     std::string serial_number = getSerialNumber (deviceIdx);
00186     if (!serial_number.empty ())
00187       serial_map_[serial_number] = deviceIdx;
00188   }
00189 
00190 
00191   // redundant, but needed for Windows right now and also for Xtion
00192   for (unsigned deviceIdx = 0; deviceIdx < device_context_.size (); ++deviceIdx)
00193   {
00194     unsigned short product_id;
00195     unsigned short vendor_id;
00196 
00197     getDeviceType(device_context_[deviceIdx].device_node.GetCreationInfo (), vendor_id, product_id );
00198 
00199 #if _WIN32
00200     if (vendor_id == 0x45e)
00201     {
00202       strcpy (const_cast<char*> (device_context_[deviceIdx].device_node.GetDescription ().strVendor), "Microsoft");
00203       strcpy (const_cast<char*> (device_context_[deviceIdx].device_node.GetDescription ().strName), "Xbox NUI Camera");
00204     }
00205     else
00206 #endif
00207     if (vendor_id == 0x1d27 && device_context_[deviceIdx].image_node.get () == 0)
00208     {
00209       strcpy (const_cast<char*> (device_context_[deviceIdx].device_node.GetDescription ().strVendor), "ASUS");
00210       strcpy (const_cast<char*> (device_context_[deviceIdx].device_node.GetDescription ().strName), "Xtion Pro");
00211     }
00212   }
00213   return (static_cast<unsigned int> (device_context_.size ()));
00214 }
00215 
00216 void OpenNIDriver::stopAll ()
00217 {
00218   XnStatus status = context_.StopGeneratingAll ();
00219   if (status != XN_STATUS_OK)
00220     THROW_OPENNI_EXCEPTION ("stopping all streams failed. Reason: %s", xnGetStatusString (status));
00221 }
00222 
00223 OpenNIDriver::~OpenNIDriver () throw ()
00224 {
00225   // no exception during destuctor
00226   try
00227   {
00228     stopAll ();
00229   }
00230   catch (...)
00231   {
00232   }
00233 
00234 #if (XN_MINOR_VERSION >= 3)
00235   context_.Release ();
00236 #else
00237   context_.Shutdown ();
00238 #endif
00239 }
00240 
00241 boost::shared_ptr<OpenNIDevice> OpenNIDriver::createVirtualDevice (const string& path, bool repeat, bool stream) const
00242 {
00243   return boost::shared_ptr<OpenNIDevice> (new DeviceONI (context_, path, repeat, stream));
00244 }
00245 
00246 boost::shared_ptr<OpenNIDevice> 
00247 OpenNIDriver::getDeviceByIndex (unsigned index) const
00248 {
00249   using namespace std;
00250 
00251   if (index >= device_context_.size ())
00252     THROW_OPENNI_EXCEPTION ("Device index out of range. Only %d devices connected but device %d requested.", device_context_.size (), index);
00253   boost::shared_ptr<OpenNIDevice> device = device_context_[index].device.lock ();
00254   if (!device)
00255   {
00256     unsigned short vendor_id;
00257     unsigned short product_id;
00258     getDeviceType (device_context_[index].device_node.GetCreationInfo (), vendor_id, product_id );
00259 
00260     if (vendor_id == 0x45e)
00261     {
00262       device.reset (new DeviceKinect (context_, 
00263                                       device_context_[index].device_node,
00264                                       *device_context_[index].image_node, 
00265                                       *device_context_[index].depth_node,
00266                                       *device_context_[index].ir_node));
00267       device_context_[index].device = device;
00268     }
00269     else if (vendor_id == 0x1d27)
00270     {
00271       if (device_context_[index].image_node.get())
00272         device.reset (new DevicePrimesense (context_, 
00273                                             device_context_[index].device_node, 
00274                                             *device_context_[index].image_node, 
00275                                             *device_context_[index].depth_node,
00276                                             *device_context_[index].ir_node));
00277       else
00278         device.reset (new DeviceXtionPro (context_, 
00279                                           device_context_[index].device_node, 
00280                                           *device_context_[index].depth_node, 
00281                                           *device_context_[index].ir_node));
00282       device_context_[index].device = device;
00283     }
00284     else
00285     {
00286       THROW_OPENNI_EXCEPTION ("Vendor %s (%s) unknown!",
00287                               getVendorName (index), vendor_id);
00288     }
00289   }
00290   return (device);
00291 }
00292 
00293 boost::shared_ptr<OpenNIDevice>
00294 OpenNIDriver::getDeviceBySerialNumber (const std::string& serial_number) const
00295 {
00296   std::map<std::string, unsigned>::const_iterator it = serial_map_.find (serial_number);
00297 
00298   if (it != serial_map_.end ())
00299   {
00300     return getDeviceByIndex (it->second);
00301   }
00302 
00303   THROW_OPENNI_EXCEPTION ("No device with serial number \'%s\' found", serial_number.c_str ());
00304 
00305   // because of warnings!!!
00306   return (boost::shared_ptr<OpenNIDevice > (static_cast<OpenNIDevice*> (NULL)));
00307 }
00308 #ifndef _WIN32
00309 boost::shared_ptr<OpenNIDevice>
00310 OpenNIDriver::getDeviceByAddress (unsigned char bus, unsigned char address) const
00311 {
00312   std::map<unsigned char, std::map<unsigned char, unsigned> >::const_iterator busIt = bus_map_.find (bus);
00313   if (busIt != bus_map_.end ())
00314   {
00315     std::map<unsigned char, unsigned>::const_iterator devIt = busIt->second.find (address);
00316     if (devIt != busIt->second.end ())
00317     {
00318       return getDeviceByIndex (devIt->second);
00319     }
00320   }
00321 
00322   THROW_OPENNI_EXCEPTION ("No device on bus: %d @ %d found", bus, address);
00323 
00324   // because of warnings!!!
00325   return (boost::shared_ptr<OpenNIDevice > (static_cast<OpenNIDevice*> (NULL)));
00326 }
00327 
00328 void
00329 OpenNIDriver::getDeviceInfos () throw ()
00330 {
00331   libusb_context *context = NULL;
00332   int result;
00333   result = libusb_init (&context); //initialize a library session
00334 
00335   if (result < 0)
00336     return;
00337 
00338   libusb_device **devices;
00339   int count = static_cast<int> (libusb_get_device_list (context, &devices));
00340   if (count < 0)
00341     return;
00342 
00343   for (int devIdx = 0; devIdx < count; ++devIdx)
00344   {
00345     libusb_device* device = devices[devIdx];
00346     uint8_t busId = libusb_get_bus_number (device);
00347     std::map<unsigned char, std::map<unsigned char, unsigned> >::const_iterator busIt = bus_map_.find (busId);
00348     if (busIt == bus_map_.end ())
00349       continue;
00350 
00351     uint8_t address = libusb_get_device_address (device);
00352     std::map<unsigned char, unsigned>::const_iterator addressIt = busIt->second.find (address);
00353     if (addressIt == busIt->second.end ())
00354       continue;
00355 
00356     unsigned nodeIdx = addressIt->second;
00357     xn::NodeInfo& current_node = device_context_[nodeIdx].device_node;
00358     XnProductionNodeDescription& description = const_cast<XnProductionNodeDescription&>(current_node.GetDescription ());
00359 
00360     libusb_device_descriptor descriptor;
00361     result = libusb_get_device_descriptor (devices[devIdx], &descriptor);
00362 
00363     if (result < 0)
00364     {
00365       strcpy (description.strVendor, "unknown");
00366       strcpy (description.strName, "unknown");
00367       current_node.SetInstanceName ("");
00368     }
00369     else
00370     {
00371       libusb_device_handle* dev_handle;
00372       result = libusb_open (device, &dev_handle);
00373       if (result < 0)
00374       {
00375         strcpy (description.strVendor, "unknown");
00376         strcpy (description.strName, "unknown");
00377         current_node.SetInstanceName ("");
00378       }
00379       else
00380       {
00381         unsigned char buffer[1024];
00382         libusb_get_string_descriptor_ascii (dev_handle, descriptor.iManufacturer, buffer, 1024);
00383         strcpy (description.strVendor, reinterpret_cast<char*> (buffer));
00384 
00385         libusb_get_string_descriptor_ascii (dev_handle, descriptor.iProduct, buffer, 1024);
00386         strcpy (description.strName, reinterpret_cast<char*> (buffer));
00387 
00388         int len = libusb_get_string_descriptor_ascii (dev_handle, descriptor.iSerialNumber, buffer, 1024);
00389         if (len > 4)
00390           current_node.SetInstanceName (reinterpret_cast<char*> (buffer));
00391         else
00392           current_node.SetInstanceName ("");
00393 
00394         libusb_close (dev_handle);
00395       }
00396     }
00397   }
00398   libusb_free_device_list (devices, 1);
00399   libusb_exit (context);
00400 }
00401 #endif
00402 
00403 const char* OpenNIDriver::getSerialNumber (unsigned index) const throw ()
00404 {
00405 #ifndef _WIN32
00406   return device_context_[index].device_node.GetInstanceName ();
00407 #else
00408   std::string info = device_context_[index].device_node.GetCreationInfo();
00409   char_separator<char> sep("#");
00410   tokenizer< char_separator<char> > tokens(info, sep);
00411   tokenizer<char_separator<char>>::iterator itr = tokens.begin();
00412   itr++;
00413   itr++;
00414   std::string sn = *itr;
00415   device_context_[index].device_node.SetInstanceName(sn.c_str());
00416   return device_context_[index].device_node.GetInstanceName ();
00417 #endif
00418 }
00419 
00420 void OpenNIDriver::getDeviceType (const std::string& connectionString, unsigned short& vendorId, unsigned short& productId)
00421 {
00422 #if _WIN32
00423     // expected format: "\\?\usb#vid_[ID]&pid_[ID]#[SERIAL]#{GUID}"
00424     typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
00425     boost::char_separator<char> separators("#&_");
00426     tokenizer tokens(connectionString, separators);
00427 
00428     unsigned int tokenIndex = 0;
00429     for (tokenizer::iterator tok_iter = tokens.begin(); tok_iter != tokens.end(); ++tok_iter, tokenIndex++)
00430     {
00431         std::string tokenValue = *tok_iter;
00432 
00433         switch(tokenIndex) {
00434             case 2:    // the vendor ID
00435                 sscanf(tokenValue.c_str(), "%hx", &vendorId);
00436                 break;
00437             case 4: // the product ID
00438                 sscanf(tokenValue.c_str(), "%hx", &productId);
00439                 break;
00440         }
00441     }
00442 #else
00443     unsigned char bus;
00444     unsigned char address;
00445     sscanf (connectionString.c_str(), "%hx/%hx@%hhu/%hhu", &vendorId, &productId, &bus, &address);
00446 #endif
00447 }
00448 
00449 const char* OpenNIDriver::getConnectionString (unsigned index) const throw ()
00450 {
00451   return device_context_[index].device_node.GetCreationInfo ();
00452 }
00453 
00454 const char* OpenNIDriver::getVendorName (unsigned index) const throw ()
00455 {
00456   return device_context_[index].device_node.GetDescription ().strVendor;
00457 }
00458 
00459 const char* OpenNIDriver::getProductName (unsigned index) const throw ()
00460 {
00461   return device_context_[index].device_node.GetDescription ().strName;
00462 }
00463 
00464 unsigned short OpenNIDriver::getVendorID (unsigned index) const throw ()
00465 {
00466   unsigned short vendor_id;
00467   unsigned short product_id;
00468 #ifndef _WIN32
00469   unsigned char bus;
00470   unsigned char address;
00471   sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00472 
00473 #else
00474   getDeviceType (device_context_[index].device_node.GetCreationInfo (), vendor_id, product_id);
00475 #endif
00476   return vendor_id;
00477 }
00478 
00479 unsigned short OpenNIDriver::getProductID (unsigned index) const throw ()
00480 {
00481   unsigned short vendor_id;
00482   unsigned short product_id;
00483 #ifndef _WIN32
00484   unsigned char bus;
00485   unsigned char address;
00486   sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00487 
00488 #else
00489   getDeviceType (device_context_[index].device_node.GetCreationInfo (), vendor_id, product_id);
00490 #endif
00491   return product_id;
00492 }
00493 
00494 unsigned char OpenNIDriver::getBus (unsigned index) const throw ()
00495 {
00496   unsigned char bus = 0;
00497 #ifndef _WIN32
00498   unsigned short vendor_id;
00499   unsigned short product_id;
00500   unsigned char address;
00501   sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00502 #endif
00503   return bus;
00504 }
00505 
00506 unsigned char OpenNIDriver::getAddress (unsigned index) const throw ()
00507 {
00508   unsigned char address = 0;
00509 #ifndef _WIN32
00510   unsigned short vendor_id;
00511   unsigned short product_id;
00512   unsigned char bus;
00513   sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
00514 #endif
00515   return address;
00516 }
00517 
00518 OpenNIDriver::DeviceContext::DeviceContext (const xn::NodeInfo& device, xn::NodeInfo* image, xn::NodeInfo* depth, xn::NodeInfo* ir)
00519 : device_node (device)
00520 , image_node (image)
00521 , depth_node (depth)
00522 , ir_node (ir)
00523 , device ()
00524 {
00525 }
00526 
00527 OpenNIDriver::DeviceContext::DeviceContext (const xn::NodeInfo& device)
00528 : device_node (device)
00529 , image_node (static_cast<xn::NodeInfo*> (0))
00530 , depth_node (static_cast<xn::NodeInfo*> (0))
00531 , ir_node (static_cast<xn::NodeInfo*> (0))
00532 , device ()
00533 {
00534 }
00535 
00536 OpenNIDriver::DeviceContext::DeviceContext (const DeviceContext& other)
00537 : device_node (other.device_node)
00538 , image_node (other.image_node)
00539 , depth_node (other.depth_node)
00540 , ir_node (other.ir_node)
00541 , device (other.device)
00542 {
00543 }
00544 
00545 } // namespace
00546 #endif


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:00