openni2_device.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Author: Julius Kammerl (jkammerl@willowgarage.com)
30  */
31 
32 #include "OpenNI.h"
33 #include <PS1080.h> // For XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE property
34 
35 #include <boost/lexical_cast.hpp>
36 #include <boost/algorithm/string/replace.hpp>
37 
42 
43 #include <boost/shared_ptr.hpp>
44 #include <boost/make_shared.hpp>
45 
46 #include <string>
47 
48 namespace openni2_wrapper
49 {
50 
51 OpenNI2Device::OpenNI2Device(const std::string& device_URI) throw (OpenNI2Exception) :
52  openni_device_(),
53  ir_video_started_(false),
54  color_video_started_(false),
55  depth_video_started_(false),
56  image_registration_activated_(false),
57  use_device_time_(false)
58 {
59  openni::Status rc = openni::OpenNI::initialize();
60  if (rc != openni::STATUS_OK)
61  THROW_OPENNI_EXCEPTION("Initialize failed\n%s\n", openni::OpenNI::getExtendedError());
62 
63  openni_device_ = boost::make_shared<openni::Device>();
64 
65  if (device_URI.length() > 0)
66  {
67  rc = openni_device_->open(device_URI.c_str());
68  }
69  else
70  {
71  rc = openni_device_->open(openni::ANY_DEVICE);
72  }
73 
74  if (rc != openni::STATUS_OK)
75  THROW_OPENNI_EXCEPTION("Device open failed\n%s\n", openni::OpenNI::getExtendedError());
76 
77  device_info_ = boost::make_shared<openni::DeviceInfo>();
78  *device_info_ = openni_device_->getDeviceInfo();
79 
80  ir_frame_listener = boost::make_shared<OpenNI2FrameListener>();
81  color_frame_listener = boost::make_shared<OpenNI2FrameListener>();
82  depth_frame_listener = boost::make_shared<OpenNI2FrameListener>();
83 
84 }
85 
87 {
89 
90  shutdown();
91 
92  openni_device_->close();
93 }
94 
95 const std::string OpenNI2Device::getUri() const
96 {
97  return std::string(device_info_->getUri());
98 }
99 
100 const std::string OpenNI2Device::getVendor() const
101 {
102  return std::string(device_info_->getVendor());
103 }
104 
105 const std::string OpenNI2Device::getName() const
106 {
107  return std::string(device_info_->getName());
108 }
109 
111 {
112  return device_info_->getUsbVendorId();
113 }
114 
116 {
117  return device_info_->getUsbProductId();
118 }
119 
120 const std::string OpenNI2Device::getStringID() const
121 {
122  std::string ID_str = getName() + "_" + getVendor();
123 
124  boost::replace_all(ID_str, "/", "");
125  boost::replace_all(ID_str, ".", "");
126  boost::replace_all(ID_str, "@", "");
127 
128  return ID_str;
129 }
130 
132 {
133  return (openni_device_.get() != 0) && openni_device_->isValid();
134 }
135 
136 float OpenNI2Device::getIRFocalLength(int output_y_resolution) const
137 {
138  float focal_length = 0.0f;
140 
141  if (stream)
142  {
143  focal_length = (float)output_y_resolution / (2 * tan(stream->getVerticalFieldOfView() / 2));
144  }
145 
146  return focal_length;
147 }
148 
149 float OpenNI2Device::getColorFocalLength(int output_y_resolution) const
150 {
151  float focal_length = 0.0f;
153 
154  if (stream)
155  {
156  focal_length = (float)output_y_resolution / (2 * tan(stream->getVerticalFieldOfView() / 2));
157  }
158 
159  return focal_length;
160 }
161 
162 float OpenNI2Device::getDepthFocalLength(int output_y_resolution) const
163 {
164  float focal_length = 0.0f;
166 
167  if (stream)
168  {
169  focal_length = (float)output_y_resolution / (2 * tan(stream->getVerticalFieldOfView() / 2));
170  }
171 
172  return focal_length;
173 }
174 
176 {
177  float baseline = 0.075f;
179 
180  if (stream && stream->isPropertySupported(XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE))
181  {
182  double baseline_meters;
183  stream->getProperty(XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE, &baseline_meters); // Device specific -- from PS1080.h
184  baseline = static_cast<float>(baseline_meters * 0.01f); // baseline from cm -> meters
185  }
186  return baseline;
187 }
188 
190 {
192 
193  bool supported = false;
194 
195  std::vector<OpenNI2VideoMode>::const_iterator it = ir_video_modes_.begin();
196  std::vector<OpenNI2VideoMode>::const_iterator it_end = ir_video_modes_.end();
197 
198  while (it != it_end && !supported)
199  {
200  supported = (*it == video_mode);
201  ++it;
202  }
203 
204  return supported;
205 }
206 
208 {
210 
211  bool supported = false;
212 
213  std::vector<OpenNI2VideoMode>::const_iterator it = color_video_modes_.begin();
214  std::vector<OpenNI2VideoMode>::const_iterator it_end = color_video_modes_.end();
215 
216  while (it != it_end && !supported)
217  {
218  supported = (*it == video_mode);
219  ++it;
220  }
221 
222  return supported;
223 }
224 
226 {
228 
229  bool supported = false;
230 
231  std::vector<OpenNI2VideoMode>::const_iterator it = depth_video_modes_.begin();
232  std::vector<OpenNI2VideoMode>::const_iterator it_end = depth_video_modes_.end();
233 
234  while (it != it_end && !supported)
235  {
236  supported = (*it == video_mode);
237  ++it;
238  }
239 
240  return supported;
241 
242 }
243 
245 {
246  return openni_device_->hasSensor(openni::SENSOR_IR);
247 }
248 
250 {
251  return openni_device_->hasSensor(openni::SENSOR_COLOR);
252 }
253 
255 {
256  return openni_device_->hasSensor(openni::SENSOR_DEPTH);
257 }
258 
260 {
262 
263  if (stream)
264  {
265  stream->setMirroringEnabled(false);
266  stream->start();
267  stream->addNewFrameListener(ir_frame_listener.get());
268  ir_video_started_ = true;
269  }
270 
271 }
272 
274 {
276 
277  if (stream)
278  {
279  stream->setMirroringEnabled(false);
280  stream->start();
281  stream->addNewFrameListener(color_frame_listener.get());
282  color_video_started_ = true;
283  }
284 }
286 {
288 
289  if (stream)
290  {
291  stream->setMirroringEnabled(false);
292  stream->start();
293  stream->addNewFrameListener(depth_frame_listener.get());
294  depth_video_started_ = true;
295  }
296 }
297 
299 {
300  stopIRStream();
301  stopColorStream();
302  stopDepthStream();
303 }
304 
306 {
307  if (ir_video_stream_.get() != 0)
308  {
309  ir_video_started_ = false;
310 
311  ir_video_stream_->removeNewFrameListener(ir_frame_listener.get());
312 
313  ir_video_stream_->stop();
314  }
315 }
317 {
318  if (color_video_stream_.get() != 0)
319  {
320  color_video_started_ = false;
321 
322  color_video_stream_->removeNewFrameListener(color_frame_listener.get());
323 
324  color_video_stream_->stop();
325  }
326 }
328 {
329  if (depth_video_stream_.get() != 0)
330  {
331  depth_video_started_ = false;
332 
333  depth_video_stream_->removeNewFrameListener(depth_frame_listener.get());
334 
335  depth_video_stream_->stop();
336  }
337 }
338 
340 {
341  if (ir_video_stream_.get() != 0)
342  ir_video_stream_->destroy();
343 
344  if (color_video_stream_.get() != 0)
345  color_video_stream_->destroy();
346 
347  if (depth_video_stream_.get() != 0)
348  depth_video_stream_->destroy();
349 
350 }
351 
353 {
354  return ir_video_started_;
355 }
357 {
358  return color_video_started_;
359 }
361 {
362  return depth_video_started_;
363 }
364 
365 const std::vector<OpenNI2VideoMode>& OpenNI2Device::getSupportedIRVideoModes() const
366 {
368 
369  ir_video_modes_.clear();
370 
371  if (stream)
372  {
373  const openni::SensorInfo& sensor_info = stream->getSensorInfo();
374 
375  ir_video_modes_ = openni2_convert(sensor_info.getSupportedVideoModes());
376  }
377 
378  return ir_video_modes_;
379 }
380 
381 const std::vector<OpenNI2VideoMode>& OpenNI2Device::getSupportedColorVideoModes() const
382 {
384 
385  color_video_modes_.clear();
386 
387  if (stream)
388  {
389  const openni::SensorInfo& sensor_info = stream->getSensorInfo();
390 
391  color_video_modes_ = openni2_convert(sensor_info.getSupportedVideoModes());
392  }
393 
394  return color_video_modes_;
395 }
396 
397 const std::vector<OpenNI2VideoMode>& OpenNI2Device::getSupportedDepthVideoModes() const
398 {
400 
401  depth_video_modes_.clear();
402 
403  if (stream)
404  {
405  const openni::SensorInfo& sensor_info = stream->getSensorInfo();
406 
407  depth_video_modes_ = openni2_convert(sensor_info.getSupportedVideoModes());
408  }
409 
410  return depth_video_modes_;
411 }
412 
414 {
415  return openni_device_->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
416 }
417 
419 {
421  {
423  if (enabled)
424  {
425  openni::Status rc = openni_device_->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
426  if (rc != openni::STATUS_OK)
427  THROW_OPENNI_EXCEPTION("Enabling image registration mode failed: \n%s\n", openni::OpenNI::getExtendedError());
428  }
429  else
430  {
431  openni::Status rc = openni_device_->setImageRegistrationMode(openni::IMAGE_REGISTRATION_OFF);
432  if (rc != openni::STATUS_OK)
433  THROW_OPENNI_EXCEPTION("Enabling image registration mode failed: \n%s\n", openni::OpenNI::getExtendedError());
434  }
435  }
436 }
437 
439 {
440  openni::Status rc = openni_device_->setDepthColorSyncEnabled(enabled);
441  if (rc != openni::STATUS_OK)
442  THROW_OPENNI_EXCEPTION("Enabling depth color synchronization failed: \n%s\n", openni::OpenNI::getExtendedError());
443 }
444 
446 {
447  OpenNI2VideoMode ret;
448 
450 
451  if (stream)
452  {
453  openni::VideoMode video_mode = stream->getVideoMode();
454 
455  ret = openni2_convert(video_mode);
456  }
457  else
458  THROW_OPENNI_EXCEPTION("Could not create video stream.");
459 
460  return ret;
461 }
462 
464 {
465  OpenNI2VideoMode ret;
466 
468 
469  if (stream)
470  {
471  openni::VideoMode video_mode = stream->getVideoMode();
472 
473  ret = openni2_convert(video_mode);
474  }
475  else
476  THROW_OPENNI_EXCEPTION("Could not create video stream.");
477 
478  return ret;
479 }
480 
482 {
483  OpenNI2VideoMode ret;
484 
486 
487  if (stream)
488  {
489  openni::VideoMode video_mode = stream->getVideoMode();
490 
491  ret = openni2_convert(video_mode);
492  }
493  else
494  THROW_OPENNI_EXCEPTION("Could not create video stream.");
495 
496  return ret;
497 }
498 
500 {
502 
503  if (stream)
504  {
505  const openni::VideoMode videoMode = openni2_convert(video_mode);
506  const openni::Status rc = stream->setVideoMode(videoMode);
507  if (rc != openni::STATUS_OK)
508  THROW_OPENNI_EXCEPTION("Couldn't set IR video mode: \n%s\n", openni::OpenNI::getExtendedError());
509  }
510 }
511 
513 {
515 
516  if (stream)
517  {
518  const openni::VideoMode videoMode = openni2_convert(video_mode);
519  const openni::Status rc = stream->setVideoMode(videoMode);
520  if (rc != openni::STATUS_OK)
521  THROW_OPENNI_EXCEPTION("Couldn't set color video mode: \n%s\n", openni::OpenNI::getExtendedError());
522  }
523 }
524 
526 {
528 
529  if (stream)
530  {
531  const openni::VideoMode videoMode = openni2_convert(video_mode);
532  const openni::Status rc = stream->setVideoMode(videoMode);
533  if (rc != openni::STATUS_OK)
534  THROW_OPENNI_EXCEPTION("Couldn't set depth video mode: \n%s\n", openni::OpenNI::getExtendedError());
535  }
536 }
537 
539 {
541 
542  if (stream)
543  {
544  openni::CameraSettings* camera_seeting = stream->getCameraSettings();
545  if (camera_seeting)
546  {
547  const openni::Status rc = camera_seeting->setAutoExposureEnabled(enable);
548  if (rc != openni::STATUS_OK)
549  THROW_OPENNI_EXCEPTION("Couldn't set auto exposure: \n%s\n", openni::OpenNI::getExtendedError());
550  }
551 
552  }
553 }
555 {
557 
558  if (stream)
559  {
560  openni::CameraSettings* camera_seeting = stream->getCameraSettings();
561  if (camera_seeting)
562  {
563  const openni::Status rc = camera_seeting->setAutoWhiteBalanceEnabled(enable);
564  if (rc != openni::STATUS_OK)
565  THROW_OPENNI_EXCEPTION("Couldn't set auto white balance: \n%s\n", openni::OpenNI::getExtendedError());
566  }
567 
568  }
569 }
570 
572 {
574 
575  if (stream)
576  {
577  openni::CameraSettings* camera_settings = stream->getCameraSettings();
578  if (camera_settings)
579  {
580  const openni::Status rc = camera_settings->setExposure(exposure);
581  if (rc != openni::STATUS_OK)
582  THROW_OPENNI_EXCEPTION("Couldn't set exposure: \n%s\n", openni::OpenNI::getExtendedError());
583  }
584  }
585 }
586 
588 {
589  bool ret = false;
590 
592 
593  if (stream)
594  {
595  openni::CameraSettings* camera_seeting = stream->getCameraSettings();
596  if (camera_seeting)
597  ret = camera_seeting->getAutoExposureEnabled();
598  }
599 
600  return ret;
601 }
602 
604 {
605  bool ret = false;
606 
608 
609  if (stream)
610  {
611  openni::CameraSettings* camera_seeting = stream->getCameraSettings();
612  if (camera_seeting)
613  ret = camera_seeting->getAutoWhiteBalanceEnabled();
614  }
615 
616  return ret;
617 }
618 
620 {
621  int ret = 0;
622 
624 
625  if (stream)
626  {
627  openni::CameraSettings* camera_settings = stream->getCameraSettings();
628  if (camera_settings)
629  ret = camera_settings->getExposure();
630  }
631 
632  return ret;
633 }
634 
636 {
637  if (ir_frame_listener)
638  ir_frame_listener->setUseDeviceTimer(enable);
639 
641  color_frame_listener->setUseDeviceTimer(enable);
642 
644  depth_frame_listener->setUseDeviceTimer(enable);
645 }
646 
648 {
649  ir_frame_listener->setCallback(callback);
650 }
651 
653 {
654  color_frame_listener->setCallback(callback);
655 }
656 
658 {
659  depth_frame_listener->setCallback(callback);
660 }
661 
663 {
664  if (ir_video_stream_.get() == 0)
665  {
666  if (hasIRSensor())
667  {
668  ir_video_stream_ = boost::make_shared<openni::VideoStream>();
669 
670  const openni::Status rc = ir_video_stream_->create(*openni_device_, openni::SENSOR_IR);
671  if (rc != openni::STATUS_OK)
672  THROW_OPENNI_EXCEPTION("Couldn't create IR video stream: \n%s\n", openni::OpenNI::getExtendedError());
673  }
674  }
675  return ir_video_stream_;
676 }
677 
679 {
680  if (color_video_stream_.get() == 0)
681  {
682  if (hasColorSensor())
683  {
684  color_video_stream_ = boost::make_shared<openni::VideoStream>();
685 
686  const openni::Status rc = color_video_stream_->create(*openni_device_, openni::SENSOR_COLOR);
687  if (rc != openni::STATUS_OK)
688  THROW_OPENNI_EXCEPTION("Couldn't create color video stream: \n%s\n", openni::OpenNI::getExtendedError());
689  }
690  }
691  return color_video_stream_;
692 }
693 
695 {
696  if (depth_video_stream_.get() == 0)
697  {
698  if (hasDepthSensor())
699  {
700  depth_video_stream_ = boost::make_shared<openni::VideoStream>();
701 
702  const openni::Status rc = depth_video_stream_->create(*openni_device_, openni::SENSOR_DEPTH);
703  if (rc != openni::STATUS_OK)
704  THROW_OPENNI_EXCEPTION("Couldn't create depth video stream: \n%s\n", openni::OpenNI::getExtendedError());
705  }
706  }
707  return depth_video_stream_;
708 }
709 
710 std::ostream& operator <<(std::ostream& stream, const OpenNI2Device& device)
711 {
712 
713  stream << "Device info (" << device.getUri() << ")" << std::endl;
714  stream << " Vendor: " << device.getVendor() << std::endl;
715  stream << " Name: " << device.getName() << std::endl;
716  stream << " USB Vendor ID: " << device.getUsbVendorId() << std::endl;
717  stream << " USB Product ID: " << device.getUsbVendorId() << std::endl << std::endl;
718 
719  if (device.hasIRSensor())
720  {
721  stream << "IR sensor video modes:" << std::endl;
722  const std::vector<OpenNI2VideoMode>& video_modes = device.getSupportedIRVideoModes();
723 
724  std::vector<OpenNI2VideoMode>::const_iterator it = video_modes.begin();
725  std::vector<OpenNI2VideoMode>::const_iterator it_end = video_modes.end();
726  for (; it != it_end; ++it)
727  stream << " - " << *it << std::endl;
728  }
729  else
730  {
731  stream << "No IR sensor available" << std::endl;
732  }
733 
734  if (device.hasColorSensor())
735  {
736  stream << "Color sensor video modes:" << std::endl;
737  const std::vector<OpenNI2VideoMode>& video_modes = device.getSupportedColorVideoModes();
738 
739  std::vector<OpenNI2VideoMode>::const_iterator it = video_modes.begin();
740  std::vector<OpenNI2VideoMode>::const_iterator it_end = video_modes.end();
741  for (; it != it_end; ++it)
742  stream << " - " << *it << std::endl;
743  }
744  else
745  {
746  stream << "No Color sensor available" << std::endl;
747  }
748 
749  if (device.hasDepthSensor())
750  {
751  stream << "Depth sensor video modes:" << std::endl;
752  const std::vector<OpenNI2VideoMode>& video_modes = device.getSupportedDepthVideoModes();
753 
754  std::vector<OpenNI2VideoMode>::const_iterator it = video_modes.begin();
755  std::vector<OpenNI2VideoMode>::const_iterator it_end = video_modes.end();
756  for (; it != it_end; ++it)
757  stream << " - " << *it << std::endl;
758  }
759  else
760  {
761  stream << "No Depth sensor available" << std::endl;
762  }
763 
764  return stream;
765 }
766 
767 }
const std::string getUri() const
void setColorVideoMode(const OpenNI2VideoMode &video_mode)
std::ostream & operator<<(std::ostream &stream, const OpenNI2Device &device)
boost::function< void(sensor_msgs::ImagePtr image)> FrameCallbackFunction
boost::shared_ptr< openni::VideoStream > depth_video_stream_
void setIRVideoMode(const OpenNI2VideoMode &video_mode)
boost::shared_ptr< openni::VideoStream > color_video_stream_
const OpenNI2VideoMode getDepthVideoMode()
const OpenNI2VideoMode getIRVideoMode()
boost::shared_ptr< openni::VideoStream > getColorVideoStream() const
float getIRFocalLength(int output_y_resolution) const
std::vector< OpenNI2VideoMode > ir_video_modes_
float getColorFocalLength(int output_y_resolution) const
OpenNI2Device(const std::string &device_URI)
#define THROW_OPENNI_EXCEPTION(format,...)
const OpenNI2VideoMode getColorVideoMode()
void setDepthVideoMode(const OpenNI2VideoMode &video_mode)
std::vector< OpenNI2VideoMode > color_video_modes_
bool isColorVideoModeSupported(const OpenNI2VideoMode &video_mode) const
bool isDepthVideoModeSupported(const OpenNI2VideoMode &video_mode) const
const std::string getVendor() const
void setDepthColorSync(bool enabled)
void setAutoWhiteBalance(bool enable)
const std::string getName() const
General exception class.
const std::string getStringID() const
boost::shared_ptr< OpenNI2FrameListener > ir_frame_listener
boost::shared_ptr< OpenNI2FrameListener > color_frame_listener
boost::shared_ptr< openni::VideoStream > getIRVideoStream() const
boost::shared_ptr< OpenNI2FrameListener > depth_frame_listener
boost::shared_ptr< openni::Device > openni_device_
const std::vector< OpenNI2VideoMode > & getSupportedColorVideoModes() const
boost::shared_ptr< openni::VideoStream > getDepthVideoStream() const
bool isImageRegistrationModeSupported() const
bool isIRVideoModeSupported(const OpenNI2VideoMode &video_mode) const
boost::shared_ptr< openni::VideoStream > ir_video_stream_
boost::shared_ptr< openni::DeviceInfo > device_info_
void setImageRegistrationMode(bool enabled)
void setDepthFrameCallback(FrameCallbackFunction callback)
const OpenNI2DeviceInfo openni2_convert(const openni::DeviceInfo *pInfo)
const std::vector< OpenNI2VideoMode > & getSupportedIRVideoModes() const
void setColorFrameCallback(FrameCallbackFunction callback)
void setIRFrameCallback(FrameCallbackFunction callback)
std::vector< OpenNI2VideoMode > depth_video_modes_
const std::vector< OpenNI2VideoMode > & getSupportedDepthVideoModes() const
float getDepthFocalLength(int output_y_resolution) const


openni2_camera
Author(s): Julius Kammerl
autogenerated on Wed Feb 3 2021 03:18:41