openni2_device_manager.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 
36 
37 #include <boost/make_shared.hpp>
38 
39 #include <ros/ros.h>
40 
41 #include <set>
42 #include <string>
43 
44 #include "OpenNI.h"
45 
46 namespace openni2_wrapper
47 {
48 
50 {
51 public:
52  bool operator()(const OpenNI2DeviceInfo& di1, const OpenNI2DeviceInfo& di2)
53  {
54  return (di1.uri_.compare(di2.uri_) < 0);
55  }
56 };
57 
58 typedef std::set<OpenNI2DeviceInfo, OpenNI2DeviceInfoComparator> DeviceSet;
59 
60 class OpenNI2DeviceListener : public openni::OpenNI::DeviceConnectedListener,
61  public openni::OpenNI::DeviceDisconnectedListener,
62  public openni::OpenNI::DeviceStateChangedListener
63 {
64 public:
66  openni::OpenNI::DeviceConnectedListener(),
67  openni::OpenNI::DeviceDisconnectedListener(),
68  openni::OpenNI::DeviceStateChangedListener()
69  {
70  openni::OpenNI::addDeviceConnectedListener(this);
71  openni::OpenNI::addDeviceDisconnectedListener(this);
72  openni::OpenNI::addDeviceStateChangedListener(this);
73 
74  // get list of currently connected devices
75  openni::Array<openni::DeviceInfo> device_info_list;
76  openni::OpenNI::enumerateDevices(&device_info_list);
77 
78  for (int i = 0; i < device_info_list.getSize(); ++i)
79  {
80  onDeviceConnected(&device_info_list[i]);
81  }
82  }
83 
85  {
86  openni::OpenNI::removeDeviceConnectedListener(this);
87  openni::OpenNI::removeDeviceDisconnectedListener(this);
88  openni::OpenNI::removeDeviceStateChangedListener(this);
89  }
90 
91  virtual void onDeviceStateChanged(const openni::DeviceInfo* pInfo, openni::DeviceState state)
92  {
93  ROS_INFO("Device \"%s\" error state changed to %d\n", pInfo->getUri(), state);
94 
95  switch (state)
96  {
97  case openni::DEVICE_STATE_OK:
98  onDeviceConnected(pInfo);
99  break;
100  case openni::DEVICE_STATE_ERROR:
101  case openni::DEVICE_STATE_NOT_READY:
102  case openni::DEVICE_STATE_EOF:
103  default:
104  onDeviceDisconnected(pInfo);
105  break;
106  }
107  }
108 
109  virtual void onDeviceConnected(const openni::DeviceInfo* pInfo)
110  {
111  boost::mutex::scoped_lock l(device_mutex_);
112 
113  const OpenNI2DeviceInfo device_info_wrapped = openni2_convert(pInfo);
114 
115  ROS_INFO("Device \"%s\" found.", pInfo->getUri());
116 
117  // make sure it does not exist in set before inserting
118  device_set_.erase(device_info_wrapped);
119  device_set_.insert(device_info_wrapped);
120  }
121 
122 
123  virtual void onDeviceDisconnected(const openni::DeviceInfo* pInfo)
124  {
125  boost::mutex::scoped_lock l(device_mutex_);
126 
127  ROS_WARN("Device \"%s\" disconnected\n", pInfo->getUri());
128 
129  const OpenNI2DeviceInfo device_info_wrapped = openni2_convert(pInfo);
130  device_set_.erase(device_info_wrapped);
131  }
132 
134  {
135  boost::mutex::scoped_lock l(device_mutex_);
136 
137  boost::shared_ptr<std::vector<std::string> > result = boost::make_shared<std::vector<std::string> >();
138 
139  result->reserve(device_set_.size());
140 
141  std::set<OpenNI2DeviceInfo, OpenNI2DeviceInfoComparator>::const_iterator it;
142  std::set<OpenNI2DeviceInfo, OpenNI2DeviceInfoComparator>::const_iterator it_end = device_set_.end();
143 
144  for (it = device_set_.begin(); it != it_end; ++it)
145  result->push_back(it->uri_);
146 
147  return result;
148  }
149 
151  {
152  boost::mutex::scoped_lock l(device_mutex_);
153 
154  boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > result = boost::make_shared<std::vector<OpenNI2DeviceInfo> >();
155 
156  result->reserve(device_set_.size());
157 
158  DeviceSet::const_iterator it;
159  DeviceSet::const_iterator it_end = device_set_.end();
160 
161  for (it = device_set_.begin(); it != it_end; ++it)
162  result->push_back(*it);
163 
164  return result;
165  }
166 
168  {
169  boost::mutex::scoped_lock l(device_mutex_);
170 
171  return device_set_.size();
172  }
173 
174  boost::mutex device_mutex_;
175  DeviceSet device_set_;
176 };
177 
179 
181 
183 {
184  openni::Status rc = openni::OpenNI::initialize();
185  if (rc != openni::STATUS_OK)
186  THROW_OPENNI_EXCEPTION("Initialize failed\n%s\n", openni::OpenNI::getExtendedError());
187 
188  device_listener_ = boost::make_shared<OpenNI2DeviceListener>();
189 }
190 
192 {
193 }
194 
196 {
197  if (singelton_.get()==0)
198  singelton_ = boost::make_shared<OpenNI2DeviceManager>();
199 
200  return singelton_;
201 }
202 
204 {
205 return device_listener_->getConnectedDeviceInfos();
206 }
207 
209 {
210  return device_listener_->getConnectedDeviceURIs();
211 }
212 
214 {
215  return device_listener_->getNumOfConnectedDevices();
216 }
217 
218 std::string OpenNI2DeviceManager::getSerial(const std::string& Uri) const
219 {
220  openni::Device openni_device;
221  std::string ret;
222 
223  // we need to open the device to query the serial number
224  if (Uri.length() > 0 && openni_device.open(Uri.c_str()) == openni::STATUS_OK)
225  {
226  int serial_len = 100;
227  char serial[serial_len];
228 
229  openni::Status rc = openni_device.getProperty(openni::DEVICE_PROPERTY_SERIAL_NUMBER, serial, &serial_len);
230  if (rc == openni::STATUS_OK)
231  ret = serial;
232  else
233  {
234  THROW_OPENNI_EXCEPTION("Serial number query failed: %s", openni::OpenNI::getExtendedError());
235  }
236  // close the device again
237  openni_device.close();
238  }
239  else
240  {
241  THROW_OPENNI_EXCEPTION("Device open failed: %s", openni::OpenNI::getExtendedError());
242  }
243  return ret;
244 }
245 
247 {
248  return boost::make_shared<OpenNI2Device>("");
249 }
251 {
252  return boost::make_shared<OpenNI2Device>(device_URI);
253 }
254 
255 
256 std::ostream& operator << (std::ostream& stream, const OpenNI2DeviceManager& device_manager) {
257 
259 
260  std::vector<OpenNI2DeviceInfo>::const_iterator it;
261  std::vector<OpenNI2DeviceInfo>::const_iterator it_end = device_info->end();
262 
263  for (it = device_info->begin(); it != it_end; ++it)
264  {
265  stream << "Uri: " << it->uri_ << " (Vendor: " << it->vendor_ <<
266  ", Name: " << it->name_ <<
267  ", Vendor ID: " << it->vendor_id_ <<
268  ", Product ID: " << it->product_id_ <<
269  ")" << std::endl;
270  }
271 
272  return stream;
273 }
274 
275 
276 } //namespace openni2_wrapper
boost::shared_ptr< std::vector< std::string > > getConnectedDeviceURIs()
std::ostream & operator<<(std::ostream &stream, const OpenNI2Device &device)
std::set< OpenNI2DeviceInfo, OpenNI2DeviceInfoComparator > DeviceSet
bool operator()(const OpenNI2DeviceInfo &di1, const OpenNI2DeviceInfo &di2)
#define THROW_OPENNI_EXCEPTION(format,...)
#define ROS_WARN(...)
virtual void onDeviceDisconnected(const openni::DeviceInfo *pInfo)
boost::shared_ptr< OpenNI2Device > getAnyDevice()
static boost::shared_ptr< OpenNI2DeviceManager > getSingelton()
virtual void onDeviceStateChanged(const openni::DeviceInfo *pInfo, openni::DeviceState state)
#define ROS_INFO(...)
boost::shared_ptr< std::vector< std::string > > getConnectedDeviceURIs() const
boost::shared_ptr< OpenNI2Device > getDevice(const std::string &device_URI)
std::string getSerial(const std::string &device_URI) const
virtual void onDeviceConnected(const openni::DeviceInfo *pInfo)
boost::shared_ptr< std::vector< OpenNI2DeviceInfo > > getConnectedDeviceInfos()
const OpenNI2DeviceInfo openni2_convert(const openni::DeviceInfo *pInfo)
boost::shared_ptr< std::vector< OpenNI2DeviceInfo > > getConnectedDeviceInfos() const
static boost::shared_ptr< OpenNI2DeviceManager > singelton_


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