00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
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
00074 device_context_.clear ();
00075
00076 bus_map_.clear ();
00077 serial_map_.clear ();
00078 connection_string_map_.clear ();
00079
00080
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;
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
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
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
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
00120
00121
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
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
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
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
00163 for (unsigned deviceIdx = 0; deviceIdx < device_context_.size (); ++deviceIdx)
00164 {
00165
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
00175 getDeviceInfos ();
00176
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
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
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
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
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
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);
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);
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
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:
00436 sscanf(tokenValue.c_str(), "%hx", &vendorId);
00437 break;
00438 case 4:
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 }