openni2_driver.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_driver.h"
00033 #include "openni2_camera/openni2_exception.h"
00034 
00035 #include <sensor_msgs/image_encodings.h>
00036 #include <sensor_msgs/distortion_models.h>
00037 
00038 #include <boost/date_time/posix_time/posix_time.hpp>
00039 #include <boost/thread/thread.hpp>
00040 
00041 namespace openni2_wrapper
00042 {
00043 
00044 OpenNI2Driver::OpenNI2Driver(ros::NodeHandle& n, ros::NodeHandle& pnh) :
00045     nh_(n),
00046     pnh_(pnh),
00047     device_manager_(OpenNI2DeviceManager::getSingelton()),
00048     config_init_(false),
00049     data_skip_ir_counter_(0),
00050     data_skip_color_counter_(0),
00051     data_skip_depth_counter_ (0),
00052     ir_subscribers_(false),
00053     color_subscribers_(false),
00054     depth_subscribers_(false),
00055     depth_raw_subscribers_(false)
00056 {
00057 
00058   genVideoModeTableMap();
00059 
00060   readConfigFromParameterServer();
00061 
00062   initDevice();
00063 
00064   // Initialize dynamic reconfigure
00065   reconfigure_server_.reset(new ReconfigureServer(pnh_));
00066   reconfigure_server_->setCallback(boost::bind(&OpenNI2Driver::configCb, this, _1, _2));
00067 
00068   while (!config_init_)
00069   {
00070     ROS_DEBUG("Waiting for dynamic reconfigure configuration.");
00071     boost::this_thread::sleep(boost::posix_time::seconds(0.1));
00072   }
00073   ROS_DEBUG("Dynamic reconfigure configuration received.");
00074 
00075   advertiseROSTopics();
00076 
00077 }
00078 
00079 void OpenNI2Driver::advertiseROSTopics()
00080 {
00081 
00082   // Allow remapping namespaces rgb, ir, depth, depth_registered
00083   ros::NodeHandle color_nh(nh_, "rgb");
00084   image_transport::ImageTransport color_it(color_nh);
00085   ros::NodeHandle ir_nh(nh_, "ir");
00086   image_transport::ImageTransport ir_it(ir_nh);
00087   ros::NodeHandle depth_nh(nh_, "depth");
00088   image_transport::ImageTransport depth_it(depth_nh);
00089   ros::NodeHandle depth_raw_nh(nh_, "depth");
00090   image_transport::ImageTransport depth_raw_it(depth_raw_nh);
00091   // Advertise all published topics
00092 
00093   // Prevent connection callbacks from executing until we've set all the publishers. Otherwise
00094   // connectCb() can fire while we're advertising (say) "depth/image_raw", but before we actually
00095   // assign to pub_depth_raw_. Then pub_depth_raw_.getNumSubscribers() returns 0, and we fail to start
00096   // the depth generator.
00097   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00098 
00099   // Asus Xtion PRO does not have an RGB camera
00100   if (device_->hasColorSensor())
00101   {
00102     image_transport::SubscriberStatusCallback itssc = boost::bind(&OpenNI2Driver::colorConnectCb, this);
00103     ros::SubscriberStatusCallback rssc = boost::bind(&OpenNI2Driver::colorConnectCb, this);
00104     pub_color_ = color_it.advertiseCamera("image", 1, itssc, itssc, rssc, rssc);
00105   }
00106 
00107   if (device_->hasIRSensor())
00108   {
00109     image_transport::SubscriberStatusCallback itssc = boost::bind(&OpenNI2Driver::irConnectCb, this);
00110     ros::SubscriberStatusCallback rssc = boost::bind(&OpenNI2Driver::irConnectCb, this);
00111     pub_ir_ = ir_it.advertiseCamera("image", 1, itssc, itssc, rssc, rssc);
00112   }
00113 
00114   if (device_->hasDepthSensor())
00115   {
00116     image_transport::SubscriberStatusCallback itssc = boost::bind(&OpenNI2Driver::depthConnectCb, this);
00117     ros::SubscriberStatusCallback rssc = boost::bind(&OpenNI2Driver::depthConnectCb, this);
00118     pub_depth_raw_ = depth_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00119     pub_depth_ = depth_raw_it.advertiseCamera("image", 1, itssc, itssc, rssc, rssc);
00120   }
00121 
00123 
00124   // Pixel offset between depth and IR images.
00125   // By default assume offset of (5,4) from 9x7 correlation window.
00126   // NOTE: These are now (temporarily?) dynamically reconfigurable, to allow tweaking.
00127   //param_nh.param("depth_ir_offset_x", depth_ir_offset_x_, 5.0);
00128   //param_nh.param("depth_ir_offset_y", depth_ir_offset_y_, 4.0);
00129 
00130   // The camera names are set to [rgb|depth]_[serial#], e.g. depth_B00367707227042B.
00131   // camera_info_manager substitutes this for ${NAME} in the URL.
00132   std::string serial_number = device_->getStringID();
00133   std::string color_name, ir_name;
00134 
00135   color_name = "rgb_"   + serial_number;
00136   ir_name  = "depth_" + serial_number;
00137 
00138   // Load the saved calibrations, if they exist
00139   color_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(color_nh, color_name, color_info_url_);
00140   ir_info_manager_  = boost::make_shared<camera_info_manager::CameraInfoManager>(ir_nh,  ir_name,  ir_info_url_);
00141 
00142 }
00143 
00144 void OpenNI2Driver::configCb(Config &config, uint32_t level)
00145 {
00146   bool stream_reset = false;
00147 
00148   depth_ir_offset_x_ = config.depth_ir_offset_x;
00149   depth_ir_offset_y_ = config.depth_ir_offset_y;
00150   z_offset_mm_ = config.z_offset_mm;
00151   z_scaling_ = config.z_scaling;
00152 
00153   ir_time_offset_ = ros::Duration(config.ir_time_offset);
00154   color_time_offset_ = ros::Duration(config.color_time_offset);
00155   depth_time_offset_ = ros::Duration(config.depth_time_offset);
00156 
00157   if (lookupVideoModeFromDynConfig(config.ir_mode, ir_video_mode_)<0)
00158   {
00159     ROS_ERROR("Undefined IR video mode received from dynamic reconfigure");
00160     exit(-1);
00161   }
00162 
00163   if (lookupVideoModeFromDynConfig(config.color_mode, color_video_mode_)<0)
00164   {
00165     ROS_ERROR("Undefined color video mode received from dynamic reconfigure");
00166     exit(-1);
00167   }
00168 
00169   if (lookupVideoModeFromDynConfig(config.depth_mode, depth_video_mode_)<0)
00170   {
00171     ROS_ERROR("Undefined depth video mode received from dynamic reconfigure");
00172     exit(-1);
00173   }
00174 
00175   // assign pixel format
00176 
00177   ir_video_mode_.pixel_format_ = PIXEL_FORMAT_GRAY16;
00178   color_video_mode_.pixel_format_ = PIXEL_FORMAT_RGB888;
00179   depth_video_mode_.pixel_format_ = PIXEL_FORMAT_DEPTH_1_MM;
00180 
00181   color_depth_synchronization_ = config.color_depth_synchronization;
00182   depth_registration_ = config.depth_registration;
00183 
00184   auto_exposure_ = config.auto_exposure;
00185   auto_white_balance_ = config.auto_white_balance;
00186 
00187   use_device_time_ = config.use_device_time;
00188 
00189   data_skip_ = config.data_skip+1;
00190 
00191   applyConfigToOpenNIDevice();
00192 
00193   config_init_ = true;
00194 
00195   old_config_ = config;
00196 }
00197 
00198 void OpenNI2Driver::setIRVideoMode(const OpenNI2VideoMode& ir_video_mode)
00199 {
00200   if (device_->isIRVideoModeSupported(ir_video_mode))
00201   {
00202     if (ir_video_mode != device_->getIRVideoMode())
00203     {
00204       device_->setIRVideoMode(ir_video_mode);
00205     }
00206 
00207   }
00208   else
00209   {
00210     ROS_ERROR_STREAM("Unsupported IR video mode - " << ir_video_mode);
00211   }
00212 }
00213 void OpenNI2Driver::setColorVideoMode(const OpenNI2VideoMode& color_video_mode)
00214 {
00215   if (device_->isColorVideoModeSupported(color_video_mode))
00216   {
00217     if (color_video_mode != device_->getColorVideoMode())
00218     {
00219       device_->setColorVideoMode(color_video_mode);
00220     }
00221   }
00222   else
00223   {
00224     ROS_ERROR_STREAM("Unsupported color video mode - " << color_video_mode);
00225   }
00226 }
00227 void OpenNI2Driver::setDepthVideoMode(const OpenNI2VideoMode& depth_video_mode)
00228 {
00229   if (device_->isDepthVideoModeSupported(depth_video_mode))
00230   {
00231     if (depth_video_mode != device_->getDepthVideoMode())
00232     {
00233       device_->setDepthVideoMode(depth_video_mode);
00234     }
00235   }
00236   else
00237   {
00238     ROS_ERROR_STREAM("Unsupported depth video mode - " << depth_video_mode);
00239   }
00240 }
00241 
00242 void OpenNI2Driver::applyConfigToOpenNIDevice()
00243 {
00244 
00245   data_skip_ir_counter_ = 0;
00246   data_skip_color_counter_= 0;
00247   data_skip_depth_counter_ = 0;
00248 
00249   setIRVideoMode(ir_video_mode_);
00250   setColorVideoMode(color_video_mode_);
00251   setDepthVideoMode(depth_video_mode_);
00252 
00253   if (device_->isImageRegistrationModeSupported())
00254   {
00255     try
00256     {
00257       if (!config_init_ || (old_config_.depth_registration != depth_registration_))
00258         device_->setImageRegistrationMode(depth_registration_);
00259     }
00260     catch (const OpenNI2Exception& exception)
00261     {
00262       ROS_ERROR("Could not set image registration. Reason: %s", exception.what());
00263     }
00264   }
00265 
00266   try
00267   {
00268     if (!config_init_ || (old_config_.color_depth_synchronization != color_depth_synchronization_))
00269       device_->setDepthColorSync(color_depth_synchronization_);
00270   }
00271   catch (const OpenNI2Exception& exception)
00272   {
00273     ROS_ERROR("Could not set color depth synchronization. Reason: %s", exception.what());
00274   }
00275 
00276   try
00277   {
00278     if (!config_init_ || (old_config_.auto_exposure != auto_exposure_))
00279       device_->setAutoExposure(auto_exposure_);
00280   }
00281   catch (const OpenNI2Exception& exception)
00282   {
00283     ROS_ERROR("Could not set auto exposure. Reason: %s", exception.what());
00284   }
00285 
00286   try
00287   {
00288     if (!config_init_ || (old_config_.auto_white_balance != auto_white_balance_))
00289       device_->setAutoWhiteBalance(auto_white_balance_);
00290   }
00291   catch (const OpenNI2Exception& exception)
00292   {
00293     ROS_ERROR("Could not set auto white balance. Reason: %s", exception.what());
00294   }
00295 
00296   device_->setUseDeviceTimer(use_device_time_);
00297 
00298 }
00299 
00300 void OpenNI2Driver::colorConnectCb()
00301 {
00302   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00303 
00304   color_subscribers_ = pub_color_.getNumSubscribers() > 0;
00305 
00306   if (color_subscribers_ && !device_->isColorStreamStarted())
00307   {
00308     // Can't stream IR and RGB at the same time. Give RGB preference.
00309     if (device_->isIRStreamStarted())
00310     {
00311       ROS_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00312       ROS_INFO("Stopping IR stream.");
00313       device_->stopIRStream();
00314     }
00315 
00316     device_->setColorFrameCallback(boost::bind(&OpenNI2Driver::newColorFrameCallback, this, _1));
00317 
00318     ROS_INFO("Starting color stream.");
00319     device_->startColorStream();
00320 
00321   }
00322   else if (!color_subscribers_ && device_->isColorStreamStarted())
00323   {
00324     ROS_INFO("Stopping color stream.");
00325     device_->stopColorStream();
00326 
00327     // Start IR if it's been blocked on RGB subscribers
00328     bool need_ir = pub_ir_.getNumSubscribers() > 0;
00329     if (need_ir && !device_->isIRStreamStarted())
00330     {
00331       device_->setIRFrameCallback(boost::bind(&OpenNI2Driver::newIRFrameCallback, this, _1));
00332 
00333       ROS_INFO("Starting IR stream.");
00334       device_->startIRStream();
00335     }
00336   }
00337 }
00338 
00339 void OpenNI2Driver::depthConnectCb()
00340 {
00341   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00342 
00343   depth_subscribers_ = pub_depth_.getNumSubscribers() > 0;
00344   depth_raw_subscribers_ = pub_depth_raw_.getNumSubscribers() > 0;
00345 
00346   bool need_depth = depth_subscribers_ || depth_raw_subscribers_;
00347 
00348   if (need_depth && !device_->isDepthStreamStarted())
00349   {
00350     device_->setDepthFrameCallback(boost::bind(&OpenNI2Driver::newDepthFrameCallback, this, _1));
00351 
00352     ROS_INFO("Starting depth stream.");
00353     device_->startDepthStream();
00354   }
00355   else if (!need_depth && device_->isDepthStreamStarted())
00356   {
00357     ROS_INFO("Stopping depth stream.");
00358     device_->stopDepthStream();
00359   }
00360 }
00361 
00362 void OpenNI2Driver::irConnectCb()
00363 {
00364   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00365 
00366   ir_subscribers_ = pub_ir_.getNumSubscribers() > 0;
00367 
00368   if (ir_subscribers_ && !device_->isIRStreamStarted())
00369   {
00370     // Can't stream IR and RGB at the same time
00371     if (device_->isColorStreamStarted())
00372     {
00373       ROS_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00374     }
00375     else
00376     {
00377       device_->setIRFrameCallback(boost::bind(&OpenNI2Driver::newIRFrameCallback, this, _1));
00378 
00379       ROS_INFO("Starting IR stream.");
00380       device_->startIRStream();
00381     }
00382   }
00383   else if (!ir_subscribers_ && device_->isIRStreamStarted())
00384   {
00385     ROS_INFO("Stopping IR stream.");
00386     device_->stopIRStream();
00387   }
00388 }
00389 
00390 void OpenNI2Driver::newIRFrameCallback(sensor_msgs::ImagePtr image)
00391 {
00392   if ((++data_skip_ir_counter_)%data_skip_==0)
00393   {
00394     data_skip_ir_counter_ = 0;
00395 
00396     if (ir_subscribers_)
00397     {
00398       image->header.frame_id = ir_frame_id_;
00399       image->header.stamp = image->header.stamp + ir_time_offset_;
00400 
00401       pub_ir_.publish(image, getIRCameraInfo(image->width, image->height, image->header.stamp));
00402     }
00403   }
00404 }
00405 
00406 void OpenNI2Driver::newColorFrameCallback(sensor_msgs::ImagePtr image)
00407 {
00408   if ((++data_skip_color_counter_)%data_skip_==0)
00409   {
00410     data_skip_color_counter_ = 0;
00411 
00412     if (color_subscribers_)
00413     {
00414       image->header.frame_id = color_frame_id_;
00415       image->header.stamp = image->header.stamp + color_time_offset_;
00416 
00417       pub_color_.publish(image, getColorCameraInfo(image->width, image->height, image->header.stamp));
00418     }
00419   }
00420 }
00421 
00422 void OpenNI2Driver::newDepthFrameCallback(sensor_msgs::ImagePtr image)
00423 {
00424   if ((++data_skip_depth_counter_)%data_skip_==0)
00425   {
00426 
00427     data_skip_depth_counter_ = 0;
00428 
00429     if (depth_raw_subscribers_||depth_subscribers_)
00430     {
00431       image->header.stamp = image->header.stamp + depth_time_offset_;
00432 
00433       if (z_offset_mm_ != 0)
00434       {
00435         uint16_t* data = reinterpret_cast<uint16_t*>(&image->data[0]);
00436         for (unsigned int i = 0; i < image->width * image->height; ++i)
00437           if (data[i] != 0)
00438                 data[i] += z_offset_mm_;
00439       }
00440 
00441       if (fabs(z_scaling_ - 1.0) > 1e-6)
00442       {
00443         uint16_t* data = reinterpret_cast<uint16_t*>(&image->data[0]);
00444         for (unsigned int i = 0; i < image->width * image->height; ++i)
00445           if (data[i] != 0)
00446                 data[i] = static_cast<uint16_t>(data[i] * z_scaling_);
00447       }
00448 
00449       sensor_msgs::CameraInfoPtr cam_info;
00450 
00451       if (depth_registration_)
00452       {
00453         image->header.frame_id = color_frame_id_;
00454         cam_info = getColorCameraInfo(image->width,image->height, image->header.stamp);
00455       } else
00456       {
00457         image->header.frame_id = depth_frame_id_;
00458         cam_info = getDepthCameraInfo(image->width,image->height, image->header.stamp);
00459       }
00460 
00461       if (depth_raw_subscribers_)
00462       {
00463         pub_depth_raw_.publish(image, cam_info);
00464       }
00465 
00466       if (depth_subscribers_ )
00467       {
00468         sensor_msgs::ImageConstPtr floating_point_image = rawToFloatingPointConversion(image);
00469         pub_depth_.publish(floating_point_image, cam_info);
00470       }
00471     }
00472   }
00473 }
00474 
00475 // Methods to get calibration parameters for the various cameras
00476 sensor_msgs::CameraInfoPtr OpenNI2Driver::getDefaultCameraInfo(int width, int height, double f) const
00477 {
00478   sensor_msgs::CameraInfoPtr info = boost::make_shared<sensor_msgs::CameraInfo>();
00479 
00480   info->width  = width;
00481   info->height = height;
00482 
00483   // No distortion
00484   info->D.resize(5, 0.0);
00485   info->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00486 
00487   // Simple camera matrix: square pixels (fx = fy), principal point at center
00488   info->K.assign(0.0);
00489   info->K[0] = info->K[4] = f;
00490   info->K[2] = (width / 2) - 0.5;
00491   // Aspect ratio for the camera center on Kinect (and other devices?) is 4/3
00492   // This formula keeps the principal point the same in VGA and SXGA modes
00493   info->K[5] = (width * (3./8.)) - 0.5;
00494   info->K[8] = 1.0;
00495 
00496   // No separate rectified image plane, so R = I
00497   info->R.assign(0.0);
00498   info->R[0] = info->R[4] = info->R[8] = 1.0;
00499 
00500   // Then P=K(I|0) = (K|0)
00501   info->P.assign(0.0);
00502   info->P[0]  = info->P[5] = f; // fx, fy
00503   info->P[2]  = info->K[2];     // cx
00504   info->P[6]  = info->K[5];     // cy
00505   info->P[10] = 1.0;
00506 
00507   return info;
00508 }
00509 
00511 sensor_msgs::CameraInfoPtr OpenNI2Driver::getColorCameraInfo(int width, int height, ros::Time time) const
00512 {
00513   sensor_msgs::CameraInfoPtr info;
00514 
00515   if (color_info_manager_->isCalibrated())
00516   {
00517     info = boost::make_shared<sensor_msgs::CameraInfo>(color_info_manager_->getCameraInfo());
00518     if ( info->width != width )
00519     {
00520       // Use uncalibrated values
00521       ROS_WARN_ONCE("Image resolution doesn't match calibration of the RGB camera. Using default parameters.");
00522       info = getDefaultCameraInfo(width, height, device_->getColorFocalLength(height));
00523     }
00524   }
00525   else
00526   {
00527     // If uncalibrated, fill in default values
00528     info = getDefaultCameraInfo(width, height, device_->getColorFocalLength(height));
00529   }
00530 
00531   // Fill in header
00532   info->header.stamp    = time;
00533   info->header.frame_id = color_frame_id_;
00534 
00535   return info;
00536 }
00537 
00538 
00539 sensor_msgs::CameraInfoPtr OpenNI2Driver::getIRCameraInfo(int width, int height, ros::Time time) const
00540 {
00541   sensor_msgs::CameraInfoPtr info;
00542 
00543   if (ir_info_manager_->isCalibrated())
00544   {
00545     info = boost::make_shared<sensor_msgs::CameraInfo>(ir_info_manager_->getCameraInfo());
00546     if ( info->width != width )
00547     {
00548       // Use uncalibrated values
00549       ROS_WARN_ONCE("Image resolution doesn't match calibration of the IR camera. Using default parameters.");
00550       info = getDefaultCameraInfo(width, height, device_->getIRFocalLength(height));
00551     }
00552   }
00553   else
00554   {
00555     // If uncalibrated, fill in default values
00556     info = getDefaultCameraInfo(width, height, device_->getDepthFocalLength(height));
00557   }
00558 
00559   // Fill in header
00560   info->header.stamp    = time;
00561   info->header.frame_id = depth_frame_id_;
00562 
00563   return info;
00564 }
00565 
00566 sensor_msgs::CameraInfoPtr OpenNI2Driver::getDepthCameraInfo(int width, int height, ros::Time time) const
00567 {
00568   // The depth image has essentially the same intrinsics as the IR image, BUT the
00569   // principal point is offset by half the size of the hardware correlation window
00570   // (probably 9x9 or 9x7 in 640x480 mode). See http://www.ros.org/wiki/kinect_calibration/technical
00571 
00572   double scaling = (double)width / 640;
00573 
00574   sensor_msgs::CameraInfoPtr info = getIRCameraInfo(width, height, time);
00575   info->K[2] -= depth_ir_offset_x_*scaling; // cx
00576   info->K[5] -= depth_ir_offset_y_*scaling; // cy
00577   info->P[2] -= depth_ir_offset_x_*scaling; // cx
00578   info->P[6] -= depth_ir_offset_y_*scaling; // cy
00579 
00581   return info;
00582 }
00583 
00584 void OpenNI2Driver::readConfigFromParameterServer()
00585 {
00586   if (!pnh_.getParam("device_id", device_id_))
00587   {
00588     ROS_WARN ("~device_id is not set! Using first device.");
00589     device_id_ = "#1";
00590   }
00591 
00592   // Camera TF frames
00593   pnh_.param("ir_frame_id", ir_frame_id_, std::string("/openni_ir_optical_frame"));
00594   pnh_.param("rgb_frame_id", color_frame_id_, std::string("/openni_rgb_optical_frame"));
00595   pnh_.param("depth_frame_id", depth_frame_id_, std::string("/openni_depth_optical_frame"));
00596 
00597   ROS_DEBUG("ir_frame_id = '%s' ", ir_frame_id_.c_str());
00598   ROS_DEBUG("rgb_frame_id = '%s' ", color_frame_id_.c_str());
00599   ROS_DEBUG("depth_frame_id = '%s' ", depth_frame_id_.c_str());
00600 
00601   pnh_.param("rgb_camera_info_url", color_info_url_, std::string());
00602   pnh_.param("depth_camera_info_url", ir_info_url_, std::string());
00603 
00604 }
00605 
00606 std::string OpenNI2Driver::resolveDeviceURI(const std::string& device_id) throw(OpenNI2Exception)
00607 {
00608   std::string device_URI;
00609   boost::shared_ptr<std::vector<std::string> > available_device_URIs = 
00610     device_manager_->getConnectedDeviceURIs();
00611 
00612   // look for '#<number>' format
00613   if (device_id_.size() > 1 && device_id_[0] == '#')
00614   {
00615     std::istringstream device_number_str(device_id_.substr(1));
00616     int device_number;
00617     device_number_str >> device_number;
00618     int device_index = device_number - 1; // #1 refers to first device
00619     if (device_index >= available_device_URIs->size() || device_index < 0)
00620     {
00621       THROW_OPENNI_EXCEPTION(
00622           "Invalid device number %i, there are %zu devices connected.",
00623           device_number, available_device_URIs->size());
00624     }
00625     else
00626     {
00627       return available_device_URIs->at(device_index);
00628     }
00629   }
00630   // look for '<bus>@<number>' format
00631   //   <bus>    is usb bus id, typically start at 1
00632   //   <number> is the device number, for consistency with openni_camera, these start at 1
00633   //               although 0 specifies "any device on this bus"
00634   else if (device_id_.size() > 1 && device_id_.find('@') != std::string::npos)
00635   {
00636     // get index of @ character
00637     size_t index = device_id_.find('@');
00638     if (index <= 0)
00639     {
00640       THROW_OPENNI_EXCEPTION(
00641         "%s is not a valid device URI, you must give the bus number before the @.",
00642         device_id_.c_str());
00643     }
00644     if (index >= device_id_.size() - 1)
00645     {
00646       THROW_OPENNI_EXCEPTION(
00647         "%s is not a valid device URI, you must give a number after the @, specify 0 for first device",
00648         device_id_.c_str());
00649     }
00650 
00651     // pull out device number on bus
00652     std::istringstream device_number_str(device_id_.substr(index+1));
00653     int device_number;
00654     device_number_str >> device_number;
00655 
00656     // reorder to @<bus>
00657     std::string bus = device_id_.substr(0, index);
00658     bus.insert(0, "@");
00659 
00660     for (size_t i = 0; i < available_device_URIs->size(); ++i)
00661     {
00662       std::string s = (*available_device_URIs)[i];
00663       if (s.find(bus) != std::string::npos)
00664       {
00665         // this matches our bus, check device number
00666         --device_number;
00667         if (device_number <= 0)
00668           return s;
00669       }
00670     }
00671 
00672     THROW_OPENNI_EXCEPTION("Device not found %s", device_id_.c_str());
00673   }
00674   else
00675   {
00676     // check if the device id given matches a serial number of a connected device
00677     boost::shared_ptr<std::vector<OpenNI2DeviceInfo> > dev_infos =
00678       device_manager_->getConnectedDeviceInfos();
00679 
00680     for(std::vector<OpenNI2DeviceInfo>::iterator it = dev_infos->begin();
00681         it != dev_infos->end(); ++it)
00682     {
00683       if (device_id.size()>0 && device_id == it->serial_)
00684         return it->uri_;
00685     }
00686 
00687     // everything else is treated as device_URI directly
00688     return device_id_;
00689   }
00690 }
00691 
00692 void OpenNI2Driver::initDevice()
00693 {
00694   while (ros::ok() && !device_)
00695   {
00696     try
00697     {
00698       std::string device_URI = resolveDeviceURI(device_id_);
00699       device_ = device_manager_->getDevice(device_URI);
00700     }
00701     catch (const OpenNI2Exception& exception)
00702     {
00703       if (!device_)
00704       {
00705         ROS_INFO("No matching device found.... waiting for devices. Reason: %s", exception.what());
00706         boost::this_thread::sleep(boost::posix_time::seconds(3));
00707         continue;
00708       }
00709       else
00710       {
00711         ROS_ERROR("Could not retrieve device. Reason: %s", exception.what());
00712         exit(-1);
00713       }
00714     }
00715   }
00716 
00717   while (ros::ok() && !device_->isValid())
00718   {
00719     ROS_DEBUG("Waiting for device initialization..");
00720     boost::this_thread::sleep(boost::posix_time::seconds(0.1));
00721   }
00722 
00723 }
00724 
00725 void OpenNI2Driver::genVideoModeTableMap()
00726 {
00727   /*
00728    * #  Video modes defined by dynamic reconfigure:
00729 output_mode_enum = gen.enum([  gen.const(  "SXGA_30Hz", int_t, 1,  "1280x1024@30Hz"),
00730                                gen.const(  "SXGA_15Hz", int_t, 2,  "1280x1024@15Hz"),
00731                                gen.const(   "XGA_30Hz", int_t, 3,  "1280x720@30Hz"),
00732                                gen.const(   "XGA_15Hz", int_t, 4,  "1280x720@15Hz"),
00733                                gen.const(   "VGA_30Hz", int_t, 5,  "640x480@30Hz"),
00734                                gen.const(   "VGA_25Hz", int_t, 6,  "640x480@25Hz"),
00735                                gen.const(  "QVGA_25Hz", int_t, 7,  "320x240@25Hz"),
00736                                gen.const(  "QVGA_30Hz", int_t, 8,  "320x240@30Hz"),
00737                                gen.const(  "QVGA_60Hz", int_t, 9,  "320x240@60Hz"),
00738                                gen.const( "QQVGA_25Hz", int_t, 10, "160x120@25Hz"),
00739                                gen.const( "QQVGA_30Hz", int_t, 11, "160x120@30Hz"),
00740                                gen.const( "QQVGA_60Hz", int_t, 12, "160x120@60Hz")],
00741                                "output mode")
00742   */
00743 
00744   video_modes_lookup_.clear();
00745 
00746   OpenNI2VideoMode video_mode;
00747 
00748   // SXGA_30Hz
00749   video_mode.x_resolution_ = 1280;
00750   video_mode.y_resolution_ = 1024;
00751   video_mode.frame_rate_ = 30;
00752 
00753   video_modes_lookup_[1] = video_mode;
00754 
00755   // SXGA_15Hz
00756   video_mode.x_resolution_ = 1280;
00757   video_mode.y_resolution_ = 1024;
00758   video_mode.frame_rate_ = 15;
00759 
00760   video_modes_lookup_[2] = video_mode;
00761 
00762   // XGA_30Hz
00763   video_mode.x_resolution_ = 1280;
00764   video_mode.y_resolution_ = 720;
00765   video_mode.frame_rate_ = 30;
00766 
00767   video_modes_lookup_[3] = video_mode;
00768 
00769   // XGA_15Hz
00770   video_mode.x_resolution_ = 1280;
00771   video_mode.y_resolution_ = 720;
00772   video_mode.frame_rate_ = 15;
00773 
00774   video_modes_lookup_[4] = video_mode;
00775 
00776   // VGA_30Hz
00777   video_mode.x_resolution_ = 640;
00778   video_mode.y_resolution_ = 480;
00779   video_mode.frame_rate_ = 30;
00780 
00781   video_modes_lookup_[5] = video_mode;
00782 
00783   // VGA_25Hz
00784   video_mode.x_resolution_ = 640;
00785   video_mode.y_resolution_ = 480;
00786   video_mode.frame_rate_ = 25;
00787 
00788   video_modes_lookup_[6] = video_mode;
00789 
00790   // QVGA_25Hz
00791   video_mode.x_resolution_ = 320;
00792   video_mode.y_resolution_ = 240;
00793   video_mode.frame_rate_ = 25;
00794 
00795   video_modes_lookup_[7] = video_mode;
00796 
00797   // QVGA_30Hz
00798   video_mode.x_resolution_ = 320;
00799   video_mode.y_resolution_ = 240;
00800   video_mode.frame_rate_ = 30;
00801 
00802   video_modes_lookup_[8] = video_mode;
00803 
00804   // QVGA_60Hz
00805   video_mode.x_resolution_ = 320;
00806   video_mode.y_resolution_ = 240;
00807   video_mode.frame_rate_ = 60;
00808 
00809   video_modes_lookup_[9] = video_mode;
00810 
00811   // QQVGA_25Hz
00812   video_mode.x_resolution_ = 160;
00813   video_mode.y_resolution_ = 120;
00814   video_mode.frame_rate_ = 25;
00815 
00816   video_modes_lookup_[10] = video_mode;
00817 
00818   // QQVGA_30Hz
00819   video_mode.x_resolution_ = 160;
00820   video_mode.y_resolution_ = 120;
00821   video_mode.frame_rate_ = 30;
00822 
00823   video_modes_lookup_[11] = video_mode;
00824 
00825   // QQVGA_60Hz
00826   video_mode.x_resolution_ = 160;
00827   video_mode.y_resolution_ = 120;
00828   video_mode.frame_rate_ = 60;
00829 
00830   video_modes_lookup_[12] = video_mode;
00831 
00832 }
00833 
00834 int OpenNI2Driver::lookupVideoModeFromDynConfig(int mode_nr, OpenNI2VideoMode& video_mode)
00835 {
00836   int ret = -1;
00837 
00838   std::map<int, OpenNI2VideoMode>::const_iterator it;
00839 
00840   it = video_modes_lookup_.find(mode_nr);
00841 
00842   if (it!=video_modes_lookup_.end())
00843   {
00844     video_mode = it->second;
00845     ret = 0;
00846   }
00847 
00848   return ret;
00849 }
00850 
00851 sensor_msgs::ImageConstPtr OpenNI2Driver::rawToFloatingPointConversion(sensor_msgs::ImageConstPtr raw_image)
00852 {
00853   static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
00854 
00855   sensor_msgs::ImagePtr new_image = boost::make_shared<sensor_msgs::Image>();
00856 
00857   new_image->header = raw_image->header;
00858   new_image->width = raw_image->width;
00859   new_image->height = raw_image->height;
00860   new_image->is_bigendian = 0;
00861   new_image->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00862   new_image->step = sizeof(float)*raw_image->width;
00863 
00864   std::size_t data_size = new_image->width*new_image->height;
00865   new_image->data.resize(data_size*sizeof(float));
00866 
00867   const unsigned short* in_ptr = reinterpret_cast<const unsigned short*>(&raw_image->data[0]);
00868   float* out_ptr = reinterpret_cast<float*>(&new_image->data[0]);
00869 
00870   for (std::size_t i = 0; i<data_size; ++i, ++in_ptr, ++out_ptr)
00871   {
00872     if (*in_ptr==0 || *in_ptr==0x7FF)
00873     {
00874       *out_ptr = bad_point;
00875     } else
00876     {
00877       *out_ptr = static_cast<float>(*in_ptr)/1000.0f;
00878     }
00879   }
00880 
00881   return new_image;
00882 }
00883 
00884 }


openni2_camera
Author(s): Julius Kammerl
autogenerated on Fri Aug 28 2015 11:54:12