openni2_device_manager.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *      Author: Julius Kammerl (jkammerl@willowgarage.com)
00030  */
00031 
00032 #include "openni2_camera/openni2_device_manager.h"
00033 #include "openni2_camera/openni2_convert.h"
00034 #include "openni2_camera/openni2_device.h"
00035 #include "openni2_camera/openni2_exception.h"
00036 
00037 #include <boost/make_shared.hpp>
00038 
00039 #include <ros/ros.h>
00040 
00041 #include <set>
00042 #include <string>
00043 
00044 #include "OpenNI.h"
00045 
00046 namespace openni2_wrapper
00047 {
00048 
00049 class OpenNI2DeviceInfoComparator
00050 {
00051 public:
00052   bool operator()(const OpenNI2DeviceInfo& di1, const OpenNI2DeviceInfo& di2)
00053   {
00054     return (di1.uri_.compare(di2.uri_) < 0);
00055   }
00056 };
00057 
00058 typedef std::set<OpenNI2DeviceInfo, OpenNI2DeviceInfoComparator> DeviceSet;
00059 
00060 class OpenNI2DeviceListener : public openni::OpenNI::DeviceConnectedListener,
00061                              public openni::OpenNI::DeviceDisconnectedListener,
00062                              public openni::OpenNI::DeviceStateChangedListener
00063 {
00064 public:
00065   OpenNI2DeviceListener() :
00066       openni::OpenNI::DeviceConnectedListener(),
00067       openni::OpenNI::DeviceDisconnectedListener(),
00068       openni::OpenNI::DeviceStateChangedListener()
00069   {
00070     openni::OpenNI::addDeviceConnectedListener(this);
00071     openni::OpenNI::addDeviceDisconnectedListener(this);
00072     openni::OpenNI::addDeviceStateChangedListener(this);
00073 
00074     // get list of currently connected devices
00075     openni::Array<openni::DeviceInfo> device_info_list;
00076     openni::OpenNI::enumerateDevices(&device_info_list);
00077 
00078     for (int i = 0; i < device_info_list.getSize(); ++i)
00079     {
00080       onDeviceConnected(&device_info_list[i]);
00081     }
00082   }
00083 
00084   ~OpenNI2DeviceListener()
00085   {
00086     openni::OpenNI::removeDeviceConnectedListener(this);
00087     openni::OpenNI::removeDeviceDisconnectedListener(this);
00088     openni::OpenNI::removeDeviceStateChangedListener(this);
00089   }
00090 
00091   virtual void onDeviceStateChanged(const openni::DeviceInfo* pInfo, openni::DeviceState state)
00092   {
00093     ROS_INFO("Device \"%s\" error state changed to %d\n", pInfo->getUri(), state);
00094 
00095     switch (state)
00096     {
00097       case openni::DEVICE_STATE_OK:
00098         onDeviceConnected(pInfo);
00099         break;
00100       case openni::DEVICE_STATE_ERROR:
00101       case openni::DEVICE_STATE_NOT_READY:
00102       case openni::DEVICE_STATE_EOF:
00103       default:
00104         onDeviceDisconnected(pInfo);
00105         break;
00106     }
00107   }
00108 
00109   virtual void onDeviceConnected(const openni::DeviceInfo* pInfo)
00110   {
00111     boost::mutex::scoped_lock l(device_mutex_);
00112 
00113     const OpenNI2DeviceInfo device_info_wrapped = openni2_convert(pInfo);
00114 
00115     ROS_INFO("Device \"%s\" found.", pInfo->getUri());
00116 
00117     // make sure it does not exist in set before inserting
00118     device_set_.erase(device_info_wrapped);
00119     device_set_.insert(device_info_wrapped);
00120   }
00121 
00122 
00123   virtual void onDeviceDisconnected(const openni::DeviceInfo* pInfo)
00124   {
00125     boost::mutex::scoped_lock l(device_mutex_);
00126 
00127     ROS_WARN("Device \"%s\" disconnected\n", pInfo->getUri());
00128 
00129     const OpenNI2DeviceInfo device_info_wrapped = openni2_convert(pInfo);
00130     device_set_.erase(device_info_wrapped);
00131   }
00132 
00133   boost::shared_ptr<std::vector<std::string> > getConnectedDeviceURIs()
00134   {
00135     boost::mutex::scoped_lock l(device_mutex_);
00136 
00137     boost::shared_ptr<std::vector<std::string> > result = boost::make_shared<std::vector<std::string> >();
00138 
00139     result->reserve(device_set_.size());
00140 
00141     std::set<OpenNI2DeviceInfo, OpenNI2DeviceInfoComparator>::const_iterator it;
00142     std::set<OpenNI2DeviceInfo, OpenNI2DeviceInfoComparator>::const_iterator it_end = device_set_.end();
00143 
00144     for (it = device_set_.begin(); it != it_end; ++it)
00145       result->push_back(it->uri_);
00146 
00147     return result;
00148   }
00149 
00150   boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > getConnectedDeviceInfos()
00151   {
00152     boost::mutex::scoped_lock l(device_mutex_);
00153 
00154     boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > result = boost::make_shared<std::vector<OpenNI2DeviceInfo> >();
00155 
00156     result->reserve(device_set_.size());
00157 
00158     DeviceSet::const_iterator it;
00159     DeviceSet::const_iterator it_end = device_set_.end();
00160 
00161     for (it = device_set_.begin(); it != it_end; ++it)
00162       result->push_back(*it);
00163 
00164     return result;
00165   }
00166 
00167   std::size_t getNumOfConnectedDevices()
00168   {
00169     boost::mutex::scoped_lock l(device_mutex_);
00170 
00171     return device_set_.size();
00172   }
00173 
00174   boost::mutex device_mutex_;
00175   DeviceSet device_set_;
00176 };
00177 
00179 
00180 boost::shared_ptr<OpenNI2DeviceManager> OpenNI2DeviceManager::singelton_;
00181 
00182 OpenNI2DeviceManager::OpenNI2DeviceManager()
00183 {
00184   openni::Status rc = openni::OpenNI::initialize();
00185   if (rc != openni::STATUS_OK)
00186       THROW_OPENNI_EXCEPTION("Initialize failed\n%s\n", openni::OpenNI::getExtendedError());
00187 
00188   device_listener_ = boost::make_shared<OpenNI2DeviceListener>();
00189 }
00190 
00191 OpenNI2DeviceManager::~OpenNI2DeviceManager()
00192 {
00193 }
00194 
00195 boost::shared_ptr<OpenNI2DeviceManager> OpenNI2DeviceManager::getSingelton()
00196 {
00197   if (singelton_.get()==0)
00198     singelton_ = boost::make_shared<OpenNI2DeviceManager>();
00199 
00200   return singelton_;
00201 }
00202 
00203 boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > OpenNI2DeviceManager::getConnectedDeviceInfos() const
00204 {
00205 return device_listener_->getConnectedDeviceInfos();
00206 }
00207 
00208 boost::shared_ptr<std::vector<std::string> > OpenNI2DeviceManager::getConnectedDeviceURIs() const
00209 {
00210   return device_listener_->getConnectedDeviceURIs();
00211 }
00212 
00213 std::size_t OpenNI2DeviceManager::getNumOfConnectedDevices() const
00214 {
00215   return device_listener_->getNumOfConnectedDevices();
00216 }
00217 
00218 std::string OpenNI2DeviceManager::getSerial(const std::string& Uri) const
00219 {
00220   openni::Device openni_device;
00221   std::string ret;
00222 
00223   // we need to open the device to query the serial number
00224   if (Uri.length() > 0 && openni_device.open(Uri.c_str()) == openni::STATUS_OK)
00225   {
00226     int serial_len = 100;
00227     char serial[serial_len];
00228 
00229     openni::Status rc = openni_device.getProperty(openni::DEVICE_PROPERTY_SERIAL_NUMBER, serial, &serial_len);
00230     if (rc == openni::STATUS_OK)
00231       ret = serial;
00232     else
00233     {
00234       THROW_OPENNI_EXCEPTION("Serial number query failed: %s", openni::OpenNI::getExtendedError());
00235     }
00236     // close the device again
00237     openni_device.close();
00238   }
00239   else
00240   {
00241     THROW_OPENNI_EXCEPTION("Device open failed: %s", openni::OpenNI::getExtendedError());
00242   }
00243   return ret;
00244 }
00245 
00246 boost::shared_ptr<OpenNI2Device> OpenNI2DeviceManager::getAnyDevice()
00247 {
00248   return boost::make_shared<OpenNI2Device>("");
00249 }
00250 boost::shared_ptr<OpenNI2Device> OpenNI2DeviceManager::getDevice(const std::string& device_URI)
00251 {
00252   return boost::make_shared<OpenNI2Device>(device_URI);
00253 }
00254 
00255 
00256 std::ostream& operator << (std::ostream& stream, const OpenNI2DeviceManager& device_manager) {
00257 
00258   boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > device_info = device_manager.getConnectedDeviceInfos();
00259 
00260   std::vector<OpenNI2DeviceInfo>::const_iterator it;
00261   std::vector<OpenNI2DeviceInfo>::const_iterator it_end = device_info->end();
00262 
00263   for (it = device_info->begin(); it != it_end; ++it)
00264   {
00265     stream << "Uri: " << it->uri_ << " (Vendor: " << it->vendor_ <<
00266                                      ", Name: " << it->name_ <<
00267                                      ", Vendor ID: " << it->vendor_id_ <<
00268                                      ", Product ID: " << it->product_id_ <<
00269                                       ")" << std::endl;
00270   }
00271 
00272   return stream;
00273 }
00274 
00275 
00276 } //namespace openni2_wrapper


openni2_camera
Author(s): Julius Kammerl
autogenerated on Tue Oct 3 2017 03:16:54