openni_driver.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011 2011 Willow Garage, Inc.
5  * Suat Gedikli <gedikli@willowgarage.com>
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
42 #include <sstream>
43 #include <iostream>
44 #include <algorithm>
45 #include <locale>
46 #include <cctype>
47 #include <map>
48 
49 #ifndef _WIN32
50 #include <libusb-1.0/libusb.h>
51 #else
52 #include <boost/tokenizer.hpp>
53 #endif
54 
55 using namespace std;
56 using namespace boost;
57 
58 namespace openni_wrapper
59 {
60 
61 OpenNIDriver::OpenNIDriver () throw (OpenNIException)
62 {
63  // Initialize the Engine
64  XnStatus status = context_.Init ();
65  if (status != XN_STATUS_OK)
66  THROW_OPENNI_EXCEPTION ("initialization failed. Reason: %s", xnGetStatusString (status));
67 
68  updateDeviceList ();
69 }
70 
71 unsigned OpenNIDriver::updateDeviceList () throw ()
72 {
73  // clear current list of devices
74  device_context_.clear ();
75  // clear maps
76  bus_map_.clear ();
77  serial_map_.clear ();
78  connection_string_map_.clear ();
79 
80  // enumerate all devices
81  static xn::NodeInfoList node_info_list;
82  XnStatus status = context_.EnumerateProductionTrees (XN_NODE_TYPE_DEVICE, NULL, node_info_list);
83  if (status != XN_STATUS_OK && node_info_list.Begin () != node_info_list.End ())
84  THROW_OPENNI_EXCEPTION ("enumerating devices failed. Reason: %s", xnGetStatusString (status));
85  else if (node_info_list.Begin () == node_info_list.End ())
86  return 0; // no exception
87 
88  for (xn::NodeInfoList::Iterator nodeIt = node_info_list.Begin (); nodeIt != node_info_list.End (); ++nodeIt)
89  {
90  connection_string_map_[(*nodeIt).GetCreationInfo ()] = device_context_.size();
91  device_context_.push_back (DeviceContext (*nodeIt));
92  }
93 
94  // enumerate depth nodes
95  static xn::NodeInfoList depth_nodes;
96  status = context_.EnumerateProductionTrees (XN_NODE_TYPE_DEPTH, NULL, depth_nodes, NULL);
97  if (status != XN_STATUS_OK)
98  THROW_OPENNI_EXCEPTION ("enumerating depth generators failed. Reason: %s", xnGetStatusString (status));
99 
100  for (xn::NodeInfoList::Iterator nodeIt = depth_nodes.Begin (); nodeIt != depth_nodes.End (); ++nodeIt)
101  {
102  // check if to which device this node is assigned to
103  for (xn::NodeInfoList::Iterator neededIt = (*nodeIt).GetNeededNodes ().Begin (); neededIt != (*nodeIt).GetNeededNodes ().End (); ++neededIt)
104  {
105  if ( connection_string_map_.count ((*neededIt).GetCreationInfo ()) )
106  {
107  unsigned device_index = connection_string_map_[(*neededIt).GetCreationInfo ()];
108  device_context_[device_index].depth_node.reset (new xn::NodeInfo(*nodeIt));
109  }
110  }
111  }
112 
113  // enumerate image nodes
114  static xn::NodeInfoList image_nodes;
115  static xn::EnumerationErrors enumeration_errors;
116  status = context_.EnumerateProductionTrees (XN_NODE_TYPE_IMAGE, NULL, image_nodes, &enumeration_errors);
117  if (status != XN_STATUS_OK)
118  {
119  // the asus pro doesn't produce images, so if the error is just "can't create node of the given type", then
120  // we ignore it (that is what error code 65565 is). if this is some other error, for instance unable
121  // to access the device due to permissions, then we throw an exception. -jbinney
122  if((enumeration_errors.Begin()).Error() != 65565) {
123  XnChar strError[1024];
124  enumeration_errors.ToString(strError, 1024);
125  THROW_OPENNI_EXCEPTION("enumerating image nodes failed. Reason: %s", strError);
126  }
127  }
128 
129  for (xn::NodeInfoList::Iterator nodeIt = image_nodes.Begin (); nodeIt != image_nodes.End (); ++nodeIt)
130  {
131  // check to which device this node is assigned to
132  for (xn::NodeInfoList::Iterator neededIt = (*nodeIt).GetNeededNodes ().Begin (); neededIt != (*nodeIt).GetNeededNodes ().End (); ++neededIt)
133  {
134  if ( connection_string_map_.count ((*neededIt).GetCreationInfo ()) )
135  {
136  unsigned device_index = connection_string_map_[(*neededIt).GetCreationInfo ()];
137  device_context_[device_index].image_node.reset (new xn::NodeInfo(*nodeIt));
138  }
139  }
140  }
141 
142  // enumerate IR nodes
143  static xn::NodeInfoList ir_nodes;
144  status = context_.EnumerateProductionTrees (XN_NODE_TYPE_IR, NULL, ir_nodes, NULL);
145  if (status != XN_STATUS_OK)
146  THROW_OPENNI_EXCEPTION ("enumerating IR generators failed. Reason: %s", xnGetStatusString (status));
147 
148  for (xn::NodeInfoList::Iterator nodeIt = ir_nodes.Begin (); nodeIt != ir_nodes.End (); ++nodeIt)
149  {
150  // check if to which device this node is assigned to
151  for (xn::NodeInfoList::Iterator neededIt = (*nodeIt).GetNeededNodes ().Begin (); neededIt != (*nodeIt).GetNeededNodes ().End (); ++neededIt)
152  {
153  if ( connection_string_map_.count ((*neededIt).GetCreationInfo ()) )
154  {
155  unsigned device_index = connection_string_map_[(*neededIt).GetCreationInfo ()];
156  device_context_[device_index].ir_node.reset (new xn::NodeInfo(*nodeIt));
157  }
158  }
159  }
160 
161 #ifndef _WIN32
162  // add context object for each found device
163  for (unsigned deviceIdx = 0; deviceIdx < device_context_.size (); ++deviceIdx)
164  {
165  // register bus@address to the corresponding context object
166  unsigned short vendor_id;
167  unsigned short product_id;
168  unsigned char bus;
169  unsigned char address;
170  sscanf (device_context_[deviceIdx].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
171  bus_map_ [bus][address] = deviceIdx;
172  }
173 
174  // get additional info about connected devices like serial number, vendor name and prduct name
175  getDeviceInfos ();
176  // build serial number -> device index map
177  for (unsigned deviceIdx = 0; deviceIdx < device_context_.size (); ++deviceIdx)
178  {
179  std::string serial_number = getSerialNumber (deviceIdx);
180  if (!serial_number.empty ())
181  serial_map_[serial_number] = deviceIdx;
182  }
183 
184 #endif
185 
186  // redundant, but needed for Windows right now and also for Xtion
187  for (unsigned deviceIdx = 0; deviceIdx < device_context_.size (); ++deviceIdx)
188  {
189  unsigned short product_id;
190  unsigned short vendor_id;
191 
192  getDeviceType(device_context_[deviceIdx].device_node.GetCreationInfo (), vendor_id, product_id );
193 
194 #if _WIN32
195  if (vendor_id == 0x45e)
196  {
197  strcpy ((char*)device_context_[deviceIdx].device_node.GetDescription().strVendor, "Microsoft");
198  strcpy ((char*)device_context_[deviceIdx].device_node.GetDescription().strName, "Xbox NUI Camera");
199  }
200  else
201 #endif
202  if (vendor_id == 0x1d27 && device_context_[deviceIdx].image_node.get () == 0)
203  {
204  strcpy ((char*)device_context_[deviceIdx].device_node.GetDescription().strVendor, "ASUS");
205  strcpy ((char*)device_context_[deviceIdx].device_node.GetDescription().strName, "Xtion Pro");
206  }
207  }
208  return (device_context_.size ());
209 }
210 
211 void OpenNIDriver::stopAll () throw (OpenNIException)
212 {
213  XnStatus status = context_.StopGeneratingAll ();
214  if (status != XN_STATUS_OK)
215  THROW_OPENNI_EXCEPTION ("stopping all streams failed. Reason: %s", xnGetStatusString (status));
216 }
217 
218 OpenNIDriver::~OpenNIDriver () throw ()
219 {
220  // no exception during destuctor
221  try
222  {
223  stopAll ();
224  }
225  catch (...)
226  {
227  }
228 
229  context_.Shutdown ();
230 }
231 
232 boost::shared_ptr<OpenNIDevice> OpenNIDriver::createVirtualDevice (const string& path, bool repeat, bool stream) const throw (OpenNIException)
233 {
234  return boost::shared_ptr<OpenNIDevice> (new DeviceONI (context_, path, repeat, stream));
235 }
236 
237 void OpenNIDriver::getPrimesenseSerial(xn::NodeInfo info, char* buffer, unsigned buf_size) const throw () {
238 
239  context_.CreateProductionTree(info);
240  xn::Device device;
241 
242  if(info.GetInstance(device) != XN_STATUS_OK) {
243  THROW_OPENNI_EXCEPTION ("couldn't get device instance for reading serial no.");
244  }
245 
246  xn::DeviceIdentificationCapability d = device.GetIdentificationCap();
247 
248  d.GetSerialNumber(buffer,buf_size);
249 
250  device.Release();
251 }
252 
253 boost::shared_ptr<OpenNIDevice> OpenNIDriver::getDeviceByIndex (unsigned index) const throw (OpenNIException)
254 {
255  using namespace std;
256 
257  if (index >= device_context_.size ())
258  THROW_OPENNI_EXCEPTION ("device index out of range. only %d devices connected but device %d requested.", device_context_.size (), index);
259  boost::shared_ptr<OpenNIDevice> device = device_context_[index].device.lock ();
260  if (!device)
261  {
262  unsigned short vendor_id;
263  unsigned short product_id;
264  getDeviceType (device_context_[index].device_node.GetCreationInfo (), vendor_id, product_id );
265 
266  if (vendor_id == 0x45e)
267  {
268  device = boost::shared_ptr<OpenNIDevice > (new DeviceKinect (context_, device_context_[index].device_node,
269  *device_context_[index].image_node, *device_context_[index].depth_node,
270  *device_context_[index].ir_node));
271  device_context_[index].device = device;
272  }
273  else if (vendor_id == 0x1d27)
274  {
275  if (device_context_[index].image_node.get())
276  device = boost::shared_ptr<OpenNIDevice > (new DevicePrimesense (context_, device_context_[index].device_node,
277  *device_context_[index].image_node, *device_context_[index].depth_node,
278  *device_context_[index].ir_node));
279  else
280  device = boost::shared_ptr<OpenNIDevice > (new DeviceXtionPro (context_, device_context_[index].device_node,
281  *device_context_[index].depth_node, *device_context_[index].ir_node));
282  device_context_[index].device = device;
283  }
284  else
285  {
286  THROW_OPENNI_EXCEPTION ("vendor %s (%s) known by primesense driver, but not by ros driver. Contact maintainer of the ros driver.",
287  getVendorName (index), vendor_id);
288  }
289  }
290  return (device);
291 }
292 
293 #ifndef _WIN32
295 OpenNIDriver::getDeviceBySerialNumber (const std::string& serial_number) const throw (OpenNIException)
296 {
297  std::map<std::string, unsigned>::const_iterator it = serial_map_.find (serial_number);
298 
299  if (it != serial_map_.end ())
300  {
301  return getDeviceByIndex (it->second);
302  }
303 
304  THROW_OPENNI_EXCEPTION ("No device with serial number \'%s\' found", serial_number.c_str ());
305 
306  // because of warnings!!!
308 }
309 
311 OpenNIDriver::getDeviceByAddress (unsigned char bus, unsigned char address) const throw (OpenNIException)
312 {
313  std::map<unsigned char, std::map<unsigned char, unsigned> >::const_iterator busIt = bus_map_.find (bus);
314  if (busIt != bus_map_.end ())
315  {
316  std::map<unsigned char, unsigned>::const_iterator devIt;
317  // Interpret invalid address 0 as "open an arbitrary device on this bus"
318  if (address == 0)
319  devIt = busIt->second.begin ();
320  else
321  devIt = busIt->second.find (address);
322 
323  if (devIt != busIt->second.end ())
324  {
325  return getDeviceByIndex (devIt->second);
326  }
327  }
328 
329  THROW_OPENNI_EXCEPTION ("No device on bus: %d @ %d found", (int)bus, (int)address);
330 
331  // because of warnings!!!
333 }
334 
335 void
336 OpenNIDriver::getDeviceInfos () throw ()
337 {
338  libusb_context *context = NULL;
339  int result;
340  result = libusb_init (&context); //initialize a library session
341 
342  if (result < 0)
343  return;
344 
345  libusb_device **devices;
346  int count = libusb_get_device_list (context, &devices);
347  if (count < 0)
348  return;
349 
350  for (int devIdx = 0; devIdx < count; ++devIdx)
351  {
352  libusb_device* device = devices[devIdx];
353  uint8_t busId = libusb_get_bus_number (device);
354  std::map<unsigned char, std::map<unsigned char, unsigned> >::const_iterator busIt = bus_map_.find (busId);
355  if (busIt == bus_map_.end ())
356  continue;
357 
358  uint8_t address = libusb_get_device_address (device);
359  std::map<unsigned char, unsigned>::const_iterator addressIt = busIt->second.find (address);
360  if (addressIt == busIt->second.end ())
361  continue;
362 
363  unsigned nodeIdx = addressIt->second;
364  xn::NodeInfo& current_node = device_context_[nodeIdx].device_node;
365 
366  libusb_device_descriptor descriptor;
367  result = libusb_get_device_descriptor (devices[devIdx], &descriptor);
368 
369  if (result < 0)
370  {
371  current_node.SetInstanceName ("");
372  }
373  else
374  {
375  libusb_device_handle* dev_handle;
376  result = libusb_open (device, &dev_handle);
377  if (result < 0)
378  {
379  current_node.SetInstanceName ("");
380  }
381  else
382  {
383  unsigned char buffer[1024];
384 
385  int len = libusb_get_string_descriptor_ascii (dev_handle, descriptor.iSerialNumber, buffer, 1024);
386  if (len > 4)
387  current_node.SetInstanceName ((char*)buffer);
388  else
389  current_node.SetInstanceName ("");
390 
391  libusb_close (dev_handle);
392  }
393  }
394  }
395  libusb_free_device_list (devices, 1);
396  libusb_exit (context);
397 }
398 #endif
399 
400 const char* OpenNIDriver::getSerialNumber (unsigned index) const throw ()
401 {
402 #ifndef _WIN32
403 
404  DeviceContext con = device_context_[index];
405  const char* openni_serial = con.device_node.GetInstanceName ();
406 
407  if (strlen(openni_serial)>0 && strcmp(openni_serial, "Device1")) {
408  return openni_serial;
409  } else {
410  char *primesense_serial = (char*)malloc(XN_MAX_NAME_LENGTH); // memleak
411  getPrimesenseSerial(con.device_node, primesense_serial, XN_MAX_NAME_LENGTH);
412 
413  return primesense_serial;
414  }
415 
416 #else
417  return "";
418 #endif
419 }
420 
421 void OpenNIDriver::getDeviceType (const std::string& connectionString, unsigned short& vendorId, unsigned short& productId)
422 {
423 #if _WIN32
424  // expected format: "\\?\usb#vid_[ID]&pid_[ID]#[SERIAL]#{GUID}"
425  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
426  boost::char_separator<char> separators("#&_");
427  tokenizer tokens(connectionString, separators);
428 
429  unsigned int tokenIndex = 0;
430  for (tokenizer::iterator tok_iter = tokens.begin(); tok_iter != tokens.end(); ++tok_iter, tokenIndex++)
431  {
432  std::string tokenValue = *tok_iter;
433 
434  switch(tokenIndex) {
435  case 2: // the vendor ID
436  sscanf(tokenValue.c_str(), "%hx", &vendorId);
437  break;
438  case 4: // the product ID
439  sscanf(tokenValue.c_str(), "%hx", &productId);
440  break;
441  }
442  }
443 #else
444  unsigned char bus;
445  unsigned char address;
446  sscanf (connectionString.c_str(), "%hx/%hx@%hhu/%hhu", &vendorId, &productId, &bus, &address);
447 #endif
448 }
449 
450 const char* OpenNIDriver::getConnectionString (unsigned index) const throw ()
451 {
452  return device_context_[index].device_node.GetCreationInfo ();
453 }
454 
455 const char* OpenNIDriver::getVendorName (unsigned index) const throw ()
456 {
457  return device_context_[index].device_node.GetDescription ().strVendor;
458 }
459 
460 const char* OpenNIDriver::getProductName (unsigned index) const throw ()
461 {
462  return device_context_[index].device_node.GetDescription ().strName;
463 }
464 
465 unsigned short OpenNIDriver::getVendorID (unsigned index) const throw ()
466 {
467  unsigned short vendor_id;
468  unsigned short product_id;
469 #ifndef _WIN32
470  unsigned char bus;
471  unsigned char address;
472  sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
473 
474 #else
475  getDeviceType (device_context_[index].device_node.GetCreationInfo (), vendor_id, product_id);
476 #endif
477  return vendor_id;
478 }
479 
480 unsigned short OpenNIDriver::getProductID (unsigned index) const throw ()
481 {
482  unsigned short vendor_id;
483  unsigned short product_id;
484 #ifndef _WIN32
485  unsigned char bus;
486  unsigned char address;
487  sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
488 
489 #else
490  getDeviceType (device_context_[index].device_node.GetCreationInfo (), vendor_id, product_id);
491 #endif
492  return product_id;
493 }
494 
495 unsigned char OpenNIDriver::getBus (unsigned index) const throw ()
496 {
497  unsigned char bus = 0;
498 #ifndef _WIN32
499  unsigned short vendor_id;
500  unsigned short product_id;
501  unsigned char address;
502  sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
503 #endif
504  return bus;
505 }
506 
507 unsigned char OpenNIDriver::getAddress (unsigned index) const throw ()
508 {
509  unsigned char address = 0;
510 #ifndef _WIN32
511  unsigned short vendor_id;
512  unsigned short product_id;
513  unsigned char bus;
514  sscanf (device_context_[index].device_node.GetCreationInfo (), "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
515 #endif
516  return address;
517 }
518 
519 OpenNIDriver::DeviceContext::DeviceContext (const xn::NodeInfo& device, xn::NodeInfo* image, xn::NodeInfo* depth, xn::NodeInfo* ir)
520 : device_node (device)
521 , image_node (image)
522 , depth_node (depth)
523 , ir_node (ir)
524 {
525 }
526 
528 : device_node (device)
529 , image_node ((xn::NodeInfo*)0)
530 , depth_node ((xn::NodeInfo*)0)
531 , ir_node ((xn::NodeInfo*)0)
532 {
533 }
534 
536 : device_node (other.device_node)
537 , image_node (other.image_node)
538 , depth_node (other.depth_node)
539 , ir_node (other.ir_node)
540 , device (other.device)
541 {
542 }
543 
544 } // namespace
Error
boost::weak_ptr< OpenNIDevice > device
#define THROW_OPENNI_EXCEPTION(format,...)
boost::shared_ptr< xn::NodeInfo > image_node
Definition: openni_driver.h:97
Concrete implementation of the interface OpenNIDevice for a Primesense device.
Concrete implementation of the interface OpenNIDevice for a MS Kinect device.
General exception class.
Concrete implementation of the interface OpenNIDevice for a Asus Xtion Pro device.
string status
Definition: test_launch.py:51
boost::shared_ptr< xn::NodeInfo > ir_node
Definition: openni_driver.h:99
Concrete implementation of the interface OpenNIDevice for a virtual device playing back an ONI file...
Class representing an astract device for Primesense or MS Kinect devices.
Definition: openni_device.h:66
boost::shared_ptr< xn::NodeInfo > depth_node
Definition: openni_driver.h:98
DeviceContext(const xn::NodeInfo &device_node, xn::NodeInfo *image_node, xn::NodeInfo *depth_node, xn::NodeInfo *ir_node)


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Mon Jun 10 2019 14:15:53