driver.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011 Willow Garage, Inc.
00005  *    Suat Gedikli <gedikli@willowgarage.com>
00006  *    Patrick Michelich <michelich@willowgarage.com>
00007  *    Radu Bogdan Rusu <rusu@willowgarage.com>
00008  *
00009  *  All rights reserved.
00010  *
00011  *  Redistribution and use in source and binary forms, with or without
00012  *  modification, are permitted provided that the following conditions
00013  *  are met:
00014  *
00015  *   * Redistributions of source code must retain the above copyright
00016  *     notice, this list of conditions and the following disclaimer.
00017  *   * Redistributions in binary form must reproduce the above
00018  *     copyright notice, this list of conditions and the following
00019  *     disclaimer in the documentation and/or other materials provided
00020  *     with the distribution.
00021  *   * Neither the name of Willow Garage, Inc. nor the names of its
00022  *     contributors may be used to endorse or promote products derived
00023  *     from this software without specific prior written permission.
00024  *
00025  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00026  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00027  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00028  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00029  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00030  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00031  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00032  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00033  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00034  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00035  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00036  *  POSSIBILITY OF SUCH DAMAGE.
00037  *
00038  */
00039 #include "driver.h" 
00040 #include "openni_camera/openni_device_kinect.h"
00041 #include "openni_camera/openni_image.h"
00042 #include "openni_camera/openni_depth_image.h"
00043 #include "openni_camera/openni_ir_image.h"
00044 #include "openni_camera/openni_image_yuv_422.h"
00045 #include <sensor_msgs/image_encodings.h>
00046 #include <sensor_msgs/distortion_models.h>
00047 #include <boost/algorithm/string/replace.hpp>
00048 
00049 #include <log4cxx/logger.h>
00050 
00051 using namespace std;
00052 using namespace openni_wrapper;
00053 namespace openni_camera
00054 {
00055 inline bool operator == (const XnMapOutputMode& mode1, const XnMapOutputMode& mode2)
00056 {
00057   return (mode1.nXRes == mode2.nXRes && mode1.nYRes == mode2.nYRes && mode1.nFPS == mode2.nFPS);
00058 }
00059 inline bool operator != (const XnMapOutputMode& mode1, const XnMapOutputMode& mode2)
00060 {
00061   return !(mode1 == mode2);
00062 }
00063 
00064 DriverNodelet::~DriverNodelet ()
00065 {
00066   // If we're still stuck in initialization (e.g. can't connect to device), break out
00067   init_thread_.interrupt();
00068   init_thread_.join();
00069 
00070   // Join OpenNI wrapper threads, which call into rgbCb() etc. Those may use device_ methods,
00071   // so make sure they've finished before destroying device_.
00072   if (device_)
00073     device_->shutdown();
00074 
00077 }
00078 
00079 void DriverNodelet::onInit ()
00080 {
00081   ros::NodeHandle& param_nh = getPrivateNodeHandle(); 
00082 
00083   config_init_ = false;
00084 
00085   // Initialize the sensor, but don't start any streams yet. That happens in the connection callbacks.
00086   updateModeMaps();
00087   setupDevice();
00088 
00089   // Initialize dynamic reconfigure
00090   reconfigure_server_.reset( new ReconfigureServer(param_nh) );
00091   reconfigure_server_->setCallback(boost::bind(&DriverNodelet::configCb, this, _1, _2));
00092   
00093   // Setting up device can take awhile but onInit shouldn't block, so we spawn a
00094   // new thread to do all the initialization
00095   init_thread_ = boost::thread(boost::bind(&DriverNodelet::onInitImpl, this));
00096 }
00097 
00098 void DriverNodelet::onInitImpl ()
00099 {
00100   ros::NodeHandle& nh       = getNodeHandle();        // topics
00101   ros::NodeHandle& param_nh = getPrivateNodeHandle(); // parameters
00102 
00103   // Allow remapping namespaces rgb, ir, depth, depth_registered
00104   image_transport::ImageTransport it(nh);
00105   ros::NodeHandle rgb_nh(nh, "rgb");
00106   image_transport::ImageTransport rgb_it(rgb_nh);
00107   ros::NodeHandle ir_nh(nh, "ir");
00108   image_transport::ImageTransport ir_it(ir_nh);
00109   ros::NodeHandle depth_nh(nh, "depth");
00110   image_transport::ImageTransport depth_it(depth_nh);
00111   ros::NodeHandle depth_registered_nh(nh, "depth_registered");
00112   image_transport::ImageTransport depth_registered_it(depth_registered_nh);
00113   ros::NodeHandle projector_nh(nh, "projector");
00114 
00115   rgb_frame_counter_ = depth_frame_counter_ = ir_frame_counter_ = 0;
00116   publish_rgb_ = publish_ir_ = publish_depth_ = true;
00117 
00118 
00119   // Camera TF frames
00120   param_nh.param("rgb_frame_id",   rgb_frame_id_,   string("/openni_rgb_optical_frame"));
00121   param_nh.param("depth_frame_id", depth_frame_id_, string("/openni_depth_optical_frame"));
00122   NODELET_INFO("rgb_frame_id = '%s' ",   rgb_frame_id_.c_str());
00123   NODELET_INFO("depth_frame_id = '%s' ", depth_frame_id_.c_str());
00124 
00125   // Pixel offset between depth and IR images.
00126   // By default assume offset of (5,4) from 9x7 correlation window.
00127   // NOTE: These are now (temporarily?) dynamically reconfigurable, to allow tweaking.
00128   //param_nh.param("depth_ir_offset_x", depth_ir_offset_x_, 5.0);
00129   //param_nh.param("depth_ir_offset_y", depth_ir_offset_y_, 4.0);
00130 
00131   // The camera names are set to [rgb|depth]_[serial#], e.g. depth_B00367707227042B.
00132   // camera_info_manager substitutes this for ${NAME} in the URL.
00133   std::string serial_number = device_->getSerialNumber();
00134   std::string rgb_name, ir_name;
00135   if (serial_number.empty())
00136   {
00137     rgb_name = "rgb";
00138     ir_name  = "depth"; 
00139   }
00140   else
00141   {
00142     rgb_name = "rgb_"   + serial_number;
00143     ir_name  = "depth_" + serial_number;
00144   }
00145 
00146   std::string rgb_info_url, ir_info_url;
00147   param_nh.param("rgb_camera_info_url", rgb_info_url, std::string());
00148   param_nh.param("depth_camera_info_url", ir_info_url, std::string());
00149 
00150   // Suppress some of the output from loading camera calibrations (kinda hacky)
00151   log4cxx::LoggerPtr logger_ccp = log4cxx::Logger::getLogger("ros.camera_calibration_parsers");
00152   log4cxx::LoggerPtr logger_cim = log4cxx::Logger::getLogger("ros.camera_info_manager");
00153   logger_ccp->setLevel(log4cxx::Level::getFatal());
00154   logger_cim->setLevel(log4cxx::Level::getWarn());
00155   // Also suppress sync warnings from image_transport::CameraSubscriber. When subscribing to
00156   // depth_registered/foo with OpenNI registration disabled, the rectify nodelet for depth_registered/
00157   // will complain because it receives depth_registered/camera_info (from the register nodelet), but
00158   // the driver is not publishing depth_registered/image_raw.
00159   log4cxx::LoggerPtr logger_its = log4cxx::Logger::getLogger("ros.image_transport.sync");
00160   logger_its->setLevel(log4cxx::Level::getError());
00161   ros::console::notifyLoggerLevelsChanged();
00162   
00163   // Load the saved calibrations, if they exist
00164   rgb_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(rgb_nh, rgb_name, rgb_info_url);
00165   ir_info_manager_  = boost::make_shared<camera_info_manager::CameraInfoManager>(ir_nh,  ir_name,  ir_info_url);
00166 
00167   if (!rgb_info_manager_->isCalibrated())
00168     NODELET_WARN("Using default parameters for RGB camera calibration.");
00169   if (!ir_info_manager_->isCalibrated())
00170     NODELET_WARN("Using default parameters for IR camera calibration.");
00171 
00172   // Advertise all published topics
00173   {
00174     // Prevent connection callbacks from executing until we've set all the publishers. Otherwise
00175     // connectCb() can fire while we're advertising (say) "depth/image_raw", but before we actually
00176     // assign to pub_depth_. Then pub_depth_.getNumSubscribers() returns 0, and we fail to start
00177     // the depth generator.
00178     boost::lock_guard<boost::mutex> lock(connect_mutex_);
00179     
00180     // Asus Xtion PRO does not have an RGB camera
00181     if (device_->hasImageStream())
00182     {
00183       image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
00184       ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
00185       pub_rgb_ = rgb_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00186     }
00187 
00188     if (device_->hasIRStream())
00189     {
00190       image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::irConnectCb, this);
00191       ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::irConnectCb, this);
00192       pub_ir_ = ir_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00193     }
00194 
00195     if (device_->hasDepthStream())
00196     {
00197       image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::depthConnectCb, this);
00198       ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::depthConnectCb, this);
00199       pub_depth_ = depth_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00200       pub_projector_info_ = projector_nh.advertise<sensor_msgs::CameraInfo>("camera_info", 1, rssc, rssc);
00201       
00202       if (device_->isDepthRegistrationSupported())
00203         pub_depth_registered_ = depth_registered_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00204     }
00205   }
00206 
00207   // Create watch dog timer callback
00208   if (param_nh.getParam("time_out", time_out_) && time_out_ > 0.0)
00209   {
00210     time_stamp_ = ros::Time(0,0);
00211     watch_dog_timer_ = nh.createTimer(ros::Duration(time_out_), &DriverNodelet::watchDog, this);
00212   }
00213 }
00214 
00215 void DriverNodelet::setupDevice ()
00216 {
00217   // Initialize the openni device
00218   OpenNIDriver& driver = OpenNIDriver::getInstance ();
00219 
00220   do {
00221     driver.updateDeviceList ();
00222 
00223     if (driver.getNumberDevices () == 0)
00224     {
00225       NODELET_INFO ("No devices connected.... waiting for devices to be connected");
00226       boost::this_thread::sleep(boost::posix_time::seconds(3));
00227       continue;
00228     }
00229 
00230     NODELET_INFO ("Number devices connected: %d", driver.getNumberDevices ());
00231     for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00232     {
00233       NODELET_INFO("%u. device on bus %03u:%02u is a %s (%03x) from %s (%03x) with serial id \'%s\'",
00234                    deviceIdx + 1, driver.getBus(deviceIdx), driver.getAddress(deviceIdx),
00235                    driver.getProductName(deviceIdx), driver.getProductID(deviceIdx),
00236                    driver.getVendorName(deviceIdx), driver.getVendorID(deviceIdx),
00237                    driver.getSerialNumber(deviceIdx));
00238     }
00239 
00240     try {
00241       string device_id;
00242       int device_id_int;
00243       if (!getPrivateNodeHandle().getParam("device_id", device_id) &&
00244           !getPrivateNodeHandle().getParam("device_id", device_id_int))
00245       {
00246         
00247         NODELET_WARN ("~device_id is not set! Using first device.");
00248         device_ = driver.getDeviceByIndex (0);
00249       }
00250       else if (device_id.find ('@') != string::npos)
00251       {
00252         size_t pos = device_id.find ('@');
00253         unsigned bus = atoi (device_id.substr (0, pos).c_str ());
00254         unsigned address = atoi (device_id.substr (pos + 1, device_id.length () - pos - 1).c_str ());
00255         NODELET_INFO ("Searching for device with bus@address = %d@%d", bus, address);
00256         device_ = driver.getDeviceByAddress (bus, address);
00257       }
00258       else if (device_id[0] == '#')
00259       {
00260         unsigned index = atoi (device_id.c_str () + 1);
00261         NODELET_INFO ("Searching for device with index = %d", index);
00262         device_ = driver.getDeviceByIndex (index - 1);
00263       }
00264       else
00265       {
00266         if (device_id.empty()) // The ID passed contains only numbers
00267         {  
00268             std::stringstream ss;
00269             ss << device_id_int;
00270             device_id = ss.str();
00271         }
00272         NODELET_INFO ("Searching for device with serial number = '%s'", device_id.c_str ());
00273         device_ = driver.getDeviceBySerialNumber (device_id);
00274       }
00275     }
00276     catch (const OpenNIException& exception)
00277     {
00278       if (!device_)
00279       {
00280         NODELET_INFO ("No matching device found.... waiting for devices. Reason: %s", exception.what ());
00281         boost::this_thread::sleep(boost::posix_time::seconds(3));
00282         continue;
00283       }
00284       else
00285       {
00286         NODELET_ERROR ("Could not retrieve device. Reason: %s", exception.what ());
00287         exit (-1);
00288       }
00289     }
00290   } while (!device_);
00291 
00292   NODELET_INFO ("Opened '%s' on bus %d:%d with serial number '%s'", device_->getProductName (),
00293                 device_->getBus (), device_->getAddress (), device_->getSerialNumber ());
00294 
00295   device_->registerImageCallback(&DriverNodelet::rgbCb,   *this);
00296   device_->registerDepthCallback(&DriverNodelet::depthCb, *this);
00297   device_->registerIRCallback   (&DriverNodelet::irCb,    *this);
00298 }
00299 
00300 void DriverNodelet::rgbConnectCb()
00301 {
00302   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00303   bool need_rgb = pub_rgb_.getNumSubscribers() > 0;
00304   
00305   if (need_rgb && !device_->isImageStreamRunning())
00306   {
00307     // Can't stream IR and RGB at the same time. Give RGB preference.
00308     if (device_->isIRStreamRunning())
00309     {
00310       NODELET_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00311       device_->stopIRStream();
00312     }
00313     
00314     device_->startImageStream();
00315     startSynchronization();
00316     time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
00317   }
00318   else if (!need_rgb && device_->isImageStreamRunning())
00319   {
00320     stopSynchronization();
00321     device_->stopImageStream();
00322 
00323     // Start IR if it's been blocked on RGB subscribers
00324     bool need_ir = pub_ir_.getNumSubscribers() > 0;
00325     if (need_ir && !device_->isIRStreamRunning())
00326     {
00327       device_->startIRStream();
00328       time_stamp_ = ros::Time(0,0);
00329     }
00330   }
00331 }
00332 
00333 void DriverNodelet::depthConnectCb()
00334 {
00335   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00337   bool need_depth =
00338     device_->isDepthRegistered() ? pub_depth_registered_.getNumSubscribers() > 0 : pub_depth_.getNumSubscribers() > 0;
00340 
00341   if (need_depth && !device_->isDepthStreamRunning())
00342   {
00343     device_->startDepthStream();
00344     startSynchronization();
00345     time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
00346   }
00347   else if (!need_depth && device_->isDepthStreamRunning())
00348   {
00349     stopSynchronization();
00350     device_->stopDepthStream();
00351   }
00352 }
00353 
00354 void DriverNodelet::irConnectCb()
00355 {
00356   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00357   bool need_ir = pub_ir_.getNumSubscribers() > 0;
00358   
00359   if (need_ir && !device_->isIRStreamRunning())
00360   {
00361     // Can't stream IR and RGB at the same time
00362     if (device_->isImageStreamRunning())
00363     {
00364       NODELET_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00365     }
00366     else
00367     {
00368       device_->startIRStream();
00369       time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
00370     }
00371   }
00372   else if (!need_ir)
00373   {
00374     device_->stopIRStream();
00375   }
00376 }
00377 
00378 // If any stream is ready, publish next available image from all streams
00379 // This publishes all available data with a maximum time offset of one frame between any two sources
00380 // Need to have lock to call this, since callbacks can be in different threads
00381 void DriverNodelet::checkFrameCounters()
00382 {
00383     if (max(rgb_frame_counter_, max(depth_frame_counter_, ir_frame_counter_)) > config_.data_skip) {
00384         // Reset all counters after we trigger publish
00385         rgb_frame_counter_   = 0;
00386         depth_frame_counter_ = 0;
00387         ir_frame_counter_    = 0;
00388 
00389         // Trigger publish on all topics
00390         publish_rgb_   = true;
00391         publish_depth_ = true;
00392         publish_ir_    = true;
00393     }
00394 }
00395 
00396 void DriverNodelet::rgbCb(boost::shared_ptr<openni_wrapper::Image> image, void* cookie)
00397 {
00398   if (!config_init_)
00399     return;
00400 
00401   ros::Time time = ros::Time::now () + ros::Duration(config_.image_time_offset);
00402   time_stamp_ = time; // for watchdog
00403 
00404   bool publish = false;
00405   {
00406       boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00407       rgb_frame_counter_++;
00408       checkFrameCounters();
00409       publish = publish_rgb_;
00410 
00411       if (publish)
00412           rgb_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
00413   }
00414 
00415   if (publish)
00416       publishRgbImage(*image, time);
00417 
00418   publish_rgb_ = false;
00419 }
00420 
00421 void DriverNodelet::depthCb(boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void* cookie)
00422 {
00423   if (!config_init_)
00424     return;
00425 
00426   ros::Time time = ros::Time::now () + ros::Duration(config_.depth_time_offset);
00427   time_stamp_ = time; // for watchdog
00428 
00429   bool publish = false;
00430   {
00431       boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00432       depth_frame_counter_++;
00433       checkFrameCounters();
00434       publish = publish_depth_;
00435 
00436       if (publish)
00437           depth_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
00438   }
00439 
00440   if (publish)
00441       publishDepthImage(*depth_image, time);
00442 
00443   publish_depth_ = false;
00444 }
00445 
00446 void DriverNodelet::irCb(boost::shared_ptr<openni_wrapper::IRImage> ir_image, void* cookie)
00447 {
00448   if (!config_init_)
00449     return;
00450 
00451   ros::Time time = ros::Time::now() + ros::Duration(config_.depth_time_offset);
00452   time_stamp_ = time; // for watchdog
00453 
00454   bool publish = false;
00455   {
00456       boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00457       ir_frame_counter_++;
00458       checkFrameCounters();
00459       publish = publish_ir_;
00460 
00461       if (publish)
00462           ir_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
00463   }
00464 
00465   if (publish)
00466       publishIrImage(*ir_image, time);
00467   publish_ir_ = false;
00468 }
00469 
00470 void DriverNodelet::publishRgbImage(const openni_wrapper::Image& image, ros::Time time) const
00471 {
00472   sensor_msgs::ImagePtr rgb_msg = boost::make_shared<sensor_msgs::Image >();
00473   rgb_msg->header.stamp = time;
00474   rgb_msg->header.frame_id = rgb_frame_id_;
00475   bool downscale = false;
00476   if (image.getEncoding() == openni_wrapper::Image::BAYER_GRBG)
00477   {
00478     if (image.getWidth() == image_width_ && image.getHeight() == image_height_)
00479     {
00480       // image sizes match, we can copy directly
00481       rgb_msg->encoding = sensor_msgs::image_encodings::BAYER_GRBG8;
00482       rgb_msg->step = image_width_;
00483     }
00484     else
00485     {
00486       // image sizes missmatch, we have to downscale and de-bayer in this function
00487       rgb_msg->encoding = sensor_msgs::image_encodings::RGB8;
00488       rgb_msg->step = image_width_ * 3;
00489       downscale = true;
00490     }
00491   }
00492   else if (image.getEncoding() == openni_wrapper::Image::YUV422)
00493   {
00494     if (image.getWidth() == image_width_ && image.getHeight() == image_height_)
00495     {
00496       // image sizes match, we can copy directly
00497       rgb_msg->encoding = sensor_msgs::image_encodings::YUV422;
00498       rgb_msg->step = image_width_ * 2; // 4 bytes for 2 pixels
00499     }
00500     else
00501     {
00502       // image sizes missmatch, we have to downscale and convert in this function
00503       rgb_msg->encoding = sensor_msgs::image_encodings::RGB8;
00504       rgb_msg->step = image_width_ * 3;
00505       downscale = true;
00506     }
00507   }
00508   rgb_msg->height = image_height_;
00509   rgb_msg->width = image_width_;
00510   rgb_msg->data.resize(rgb_msg->height * rgb_msg->step);
00511   
00512   try
00513   {
00514     if (downscale)
00515     {
00516       if (image.getEncoding() == openni_wrapper::Image::BAYER_GRBG)
00517       {
00518         openni_wrapper::ImageBayerGRBG bayer_image(image.getMetaDataPtr(), openni_wrapper::ImageBayerGRBG::Bilinear);
00519         bayer_image.fillRGB(image_width_, image_height_, &rgb_msg->data[0]);
00520       }
00521       else if (image.getEncoding() == openni_wrapper::Image::YUV422)
00522       {
00523         openni_wrapper::ImageYUV422 yuv_image(image.getMetaDataPtr());
00524         yuv_image.fillRGB(image_width_, image_height_, &rgb_msg->data[0]);
00525       }
00526     }
00527     else
00528       image.fillRaw(&rgb_msg->data[0]);
00529   }
00530   catch ( OpenNIException e )
00531   {
00532     ROS_ERROR_THROTTLE(1,"%s",e.what());
00533   }
00534   
00535   pub_rgb_.publish(rgb_msg, getRgbCameraInfo(rgb_msg->width,rgb_msg->height,time));
00536 }
00537 
00538 void DriverNodelet::publishDepthImage(const openni_wrapper::DepthImage& depth, ros::Time time) const
00539 {
00540   bool registered = device_->isDepthRegistered();
00541   
00543   sensor_msgs::ImagePtr depth_msg = boost::make_shared<sensor_msgs::Image>();
00544   depth_msg->header.stamp    = time;
00545   depth_msg->encoding        = sensor_msgs::image_encodings::TYPE_16UC1;
00546   depth_msg->width           = depth_width_;
00547   depth_msg->height          = depth_height_;
00548   depth_msg->step            = depth_msg->width * sizeof(short);
00549   depth_msg->data.resize(depth_msg->height * depth_msg->step);
00550 
00551   try
00552   {
00553     depth.fillDepthImageRaw(depth_msg->width, depth_msg->height, reinterpret_cast<unsigned short*>(&depth_msg->data[0]),
00554                             depth_msg->step);
00555   }
00556   catch ( OpenNIException e )
00557   {
00558     ROS_ERROR_THROTTLE(1,"%s",e.what());
00559   }
00560 
00561   if (fabs(z_scaling_ - 1.0) > 1e-6)
00562   {
00563     uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg->data[0]);
00564     for (unsigned int i = 0; i < depth_msg->width * depth_msg->height; ++i)
00565       if (data[i] != 0)
00566             data[i] = static_cast<uint16_t>(data[i] * z_scaling_);
00567   }
00568 
00569   if (z_offset_mm_ != 0)
00570   {
00571     uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg->data[0]);
00572     for (unsigned int i = 0; i < depth_msg->width * depth_msg->height; ++i)
00573       if (data[i] != 0)
00574             data[i] += z_offset_mm_;
00575   }
00576 
00577   if (registered)
00578   {
00579     // Publish RGB camera info and raw depth image to depth_registered/ ns
00580     depth_msg->header.frame_id = rgb_frame_id_;
00581     pub_depth_registered_.publish(depth_msg, getRgbCameraInfo(depth_msg->width, depth_msg->height, time));
00582   }
00583   else
00584   {
00585     // Publish depth camera info and raw depth image to depth/ ns
00586     depth_msg->header.frame_id = depth_frame_id_;
00587     pub_depth_.publish(depth_msg, getDepthCameraInfo(depth_msg->width, depth_msg->height, time));
00588   }
00589 
00590   // Projector "info" probably only useful for working with disparity images
00591   if (pub_projector_info_.getNumSubscribers() > 0)
00592   {
00593     pub_projector_info_.publish(getProjectorCameraInfo(depth_msg->width, depth_msg->height, time));
00594   }
00595 }
00596 
00597 void DriverNodelet::publishIrImage(const openni_wrapper::IRImage& ir, ros::Time time) const
00598 {
00599   sensor_msgs::ImagePtr ir_msg = boost::make_shared<sensor_msgs::Image>();
00600   ir_msg->header.stamp    = time;
00601   ir_msg->header.frame_id = depth_frame_id_;
00602   ir_msg->encoding        = sensor_msgs::image_encodings::MONO16;
00603   ir_msg->height          = ir.getHeight();
00604   ir_msg->width           = ir.getWidth();
00605   ir_msg->step            = ir_msg->width * sizeof(uint16_t);
00606   ir_msg->data.resize(ir_msg->height * ir_msg->step);
00607 
00608   try
00609   {
00610     ir.fillRaw(ir.getWidth(), ir.getHeight(), reinterpret_cast<unsigned short*>(&ir_msg->data[0]));
00611   }
00612   catch ( OpenNIException e )
00613   {
00614     ROS_ERROR_THROTTLE(1,"%s",e.what());
00615   }
00616 
00617   pub_ir_.publish(ir_msg, getIrCameraInfo(ir.getWidth(), ir.getHeight(), time));
00618 }
00619 
00620 sensor_msgs::CameraInfoPtr DriverNodelet::getDefaultCameraInfo(int width, int height, double f) const
00621 {
00622   sensor_msgs::CameraInfoPtr info = boost::make_shared<sensor_msgs::CameraInfo>();
00623 
00624   info->width  = width;
00625   info->height = height;
00626 
00627   // No distortion
00628   info->D.resize(5, 0.0);
00629   info->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00630 
00631   // Simple camera matrix: square pixels (fx = fy), principal point at center
00632   info->K.assign(0.0);
00633   info->K[0] = info->K[4] = f;
00634   info->K[2] = (width / 2) - 0.5;
00635   // Aspect ratio for the camera center on Kinect (and other devices?) is 4/3
00636   // This formula keeps the principal point the same in VGA and SXGA modes
00637   info->K[5] = (width * (3./8.)) - 0.5;
00638   info->K[8] = 1.0;
00639 
00640   // No separate rectified image plane, so R = I
00641   info->R.assign(0.0);
00642   info->R[0] = info->R[4] = info->R[8] = 1.0;
00643 
00644   // Then P=K(I|0) = (K|0)
00645   info->P.assign(0.0);
00646   info->P[0]  = info->P[5] = f; // fx, fy
00647   info->P[2]  = info->K[2];     // cx
00648   info->P[6]  = info->K[5];     // cy
00649   info->P[10] = 1.0;
00650 
00651   return info;
00652 }
00653 
00655 sensor_msgs::CameraInfoPtr DriverNodelet::getRgbCameraInfo(int width, int height, ros::Time time) const
00656 {
00657   sensor_msgs::CameraInfoPtr info;
00658 
00659   if (rgb_info_manager_->isCalibrated())
00660   {
00661     info = boost::make_shared<sensor_msgs::CameraInfo>(rgb_info_manager_->getCameraInfo());
00662     if ( info->width != width )
00663     {
00664       // Use uncalibrated values
00665       ROS_WARN_ONCE("Image resolution doesn't match calibration of the RGB camera. Using default parameters.");
00666       info = getDefaultCameraInfo(width, height, device_->getImageFocalLength(width));
00667     }
00668   }
00669   else
00670   {
00671     // If uncalibrated, fill in default values
00672     info = getDefaultCameraInfo(width, height, device_->getImageFocalLength(width));
00673   }
00674 
00675   // Fill in header
00676   info->header.stamp    = time;
00677   info->header.frame_id = rgb_frame_id_;
00678 
00679   return info;
00680 }
00681 
00682 sensor_msgs::CameraInfoPtr DriverNodelet::getIrCameraInfo(int width, int height, ros::Time time) const
00683 {
00684   sensor_msgs::CameraInfoPtr info;
00685 
00686   if (ir_info_manager_->isCalibrated())
00687   {
00688     info = boost::make_shared<sensor_msgs::CameraInfo>(ir_info_manager_->getCameraInfo());
00689     if ( info->width != width )
00690     {
00691       // Use uncalibrated values
00692       ROS_WARN_ONCE("Image resolution doesn't match calibration of the IR camera. Using default parameters.");
00693       info = getDefaultCameraInfo(width, height, device_->getImageFocalLength(width));
00694     }
00695   }
00696   else
00697   {
00698     // If uncalibrated, fill in default values
00699     info = getDefaultCameraInfo(width, height, device_->getDepthFocalLength(width));
00700   }
00701 
00702   // Fill in header
00703   info->header.stamp    = time;
00704   info->header.frame_id = depth_frame_id_;
00705 
00706   return info;
00707 }
00708 
00709 sensor_msgs::CameraInfoPtr DriverNodelet::getDepthCameraInfo(int width, int height, ros::Time time) const
00710 {
00711   // The depth image has essentially the same intrinsics as the IR image, BUT the
00712   // principal point is offset by half the size of the hardware correlation window
00713   // (probably 9x9 or 9x7 in 640x480 mode). See http://www.ros.org/wiki/kinect_calibration/technical
00714 
00715   double scaling = (double)width / 640;
00716 
00717   sensor_msgs::CameraInfoPtr info = getIrCameraInfo(width, height, time);
00718   info->K[2] -= depth_ir_offset_x_*scaling; // cx
00719   info->K[5] -= depth_ir_offset_y_*scaling; // cy
00720   info->P[2] -= depth_ir_offset_x_*scaling; // cx
00721   info->P[6] -= depth_ir_offset_y_*scaling; // cy
00722 
00724   return info;
00725 }
00726 
00727 sensor_msgs::CameraInfoPtr DriverNodelet::getProjectorCameraInfo(int width, int height, ros::Time time) const
00728 {
00729   // The projector info is simply the depth info with the baseline encoded in the P matrix.
00730   // It's only purpose is to be the "right" camera info to the depth camera's "left" for
00731   // processing disparity images.
00732   sensor_msgs::CameraInfoPtr info = getDepthCameraInfo(width, height, time);
00733   // Tx = -baseline * fx
00734   info->P[3] = -device_->getBaseline() * info->P[0];
00735   return info;
00736 }
00737 
00738 void DriverNodelet::configCb(Config &config, uint32_t level)
00739 {
00740   depth_ir_offset_x_ = config.depth_ir_offset_x;
00741   depth_ir_offset_y_ = config.depth_ir_offset_y;
00742   z_offset_mm_ = config.z_offset_mm;
00743   z_scaling_ = config.z_scaling;
00744 
00745   // Based on the config options, try to set the depth/image mode
00746   // The device driver might decide to switch to a 'compatible mode',
00747   // i.e. a higher resolution than the requested one, in which case it
00748   // will later downsample the image.
00749   // Only if the 'compatible mode' is different from the current one, we will
00750   // need to reset the streams.
00751 
00752   bool depth_mode_changed = false;
00753   bool image_mode_changed = false;
00754 
00755   XnMapOutputMode compatible_image_mode;
00756   XnMapOutputMode compatible_depth_mode;
00757 
00758   if (device_->hasImageStream ())
00759   {
00760     XnMapOutputMode image_mode = mapConfigMode2XnMode (config.image_mode);
00761     image_width_  = image_mode.nXRes;
00762     image_height_ = image_mode.nYRes;
00763 
00764     // does the device support the new image mode?
00765     if (!device_->findCompatibleImageMode (image_mode, compatible_image_mode))
00766     {
00767       XnMapOutputMode default_mode = device_->getDefaultImageMode();
00768       NODELET_WARN("Could not find any compatible image output mode for %d x %d @ %d. "
00769                    "Falling back to default image output mode %d x %d @ %d.",
00770                     image_mode.nXRes, image_mode.nYRes, image_mode.nFPS,
00771                     default_mode.nXRes, default_mode.nYRes, default_mode.nFPS);
00772 
00773       config.image_mode = mapXnMode2ConfigMode(default_mode);
00774       image_mode = compatible_image_mode = default_mode;
00775     }
00776 
00777     if ( compatible_image_mode != device_->getImageOutputMode () )
00778     {
00779       image_mode_changed = true;
00780     }
00781   }
00782   
00783   if (device_->hasDepthStream())
00784   {
00785     XnMapOutputMode depth_mode = mapConfigMode2XnMode (config.depth_mode);
00786     depth_width_  = depth_mode.nXRes;
00787     depth_height_ = depth_mode.nYRes;
00788 
00789     // does the device support the new depth mode?
00790     if (!device_->findCompatibleDepthMode (depth_mode, compatible_depth_mode))
00791     {
00792       XnMapOutputMode default_mode = device_->getDefaultDepthMode();
00793       NODELET_WARN ("Could not find any compatible depth output mode for %d x %d @ %d. "
00794                     "Falling back to default depth output mode %d x %d @ %d.",
00795                     depth_mode.nXRes, depth_mode.nYRes, depth_mode.nFPS,
00796                     default_mode.nXRes, default_mode.nYRes, default_mode.nFPS);
00797 
00798       config.depth_mode = mapXnMode2ConfigMode(default_mode);
00799       depth_mode = compatible_depth_mode = default_mode;
00800     }
00801 
00802     if ( compatible_depth_mode != device_->getDepthOutputMode () )
00803     {
00804       depth_mode_changed = true;
00805     }
00806   }
00807 
00808   // here everything is fine. Now make the changes
00809   if ( image_mode_changed || depth_mode_changed )
00810   {
00811     // streams need to be reset!
00812     stopSynchronization();
00813 
00814     if ( image_mode_changed )
00815     {
00816       device_->setImageOutputMode (compatible_image_mode);
00817     }
00818 
00819     if ( depth_mode_changed )
00820     {
00821       device_->setDepthOutputMode (compatible_depth_mode);
00822     }
00823 
00824     startSynchronization ();
00825   }
00826 
00828   if (device_->isDepthRegistered () && !config.depth_registration)
00829   {
00830     device_->setDepthRegistration (false);
00831   }
00832   else if (!device_->isDepthRegistered () && config.depth_registration)
00833   {
00834     device_->setDepthRegistration (true);
00835   }
00836 
00837   // now we can copy
00838   config_ = config;
00839 
00840   config_init_ = true;
00841 }
00842 
00843 void DriverNodelet::startSynchronization()
00844 {
00845   if (device_->isSynchronizationSupported() &&
00846       !device_->isSynchronized() &&
00847       device_->getImageOutputMode().nFPS == device_->getDepthOutputMode().nFPS &&
00848       device_->isImageStreamRunning() &&
00849       device_->isDepthStreamRunning() )
00850   {
00851     device_->setSynchronization(true);
00852   }
00853 }
00854 
00855 void DriverNodelet::stopSynchronization()
00856 {
00857   if (device_->isSynchronizationSupported() &&
00858       device_->isSynchronized())
00859   {
00860     device_->setSynchronization(false);
00861   }
00862 }
00863 
00864 void DriverNodelet::updateModeMaps ()
00865 {
00866   XnMapOutputMode output_mode;
00867 
00868   output_mode.nXRes = XN_SXGA_X_RES;
00869   output_mode.nYRes = XN_SXGA_Y_RES;
00870   output_mode.nFPS  = 15;
00871   xn2config_map_[output_mode] = OpenNI_SXGA_15Hz;
00872   config2xn_map_[OpenNI_SXGA_15Hz] = output_mode;
00873 
00874   output_mode.nXRes = XN_VGA_X_RES;
00875   output_mode.nYRes = XN_VGA_Y_RES;
00876   output_mode.nFPS  = 25;
00877   xn2config_map_[output_mode] = OpenNI_VGA_25Hz;
00878   config2xn_map_[OpenNI_VGA_25Hz] = output_mode;
00879   output_mode.nFPS  = 30;
00880   xn2config_map_[output_mode] = OpenNI_VGA_30Hz;
00881   config2xn_map_[OpenNI_VGA_30Hz] = output_mode;
00882 
00883   output_mode.nXRes = XN_QVGA_X_RES;
00884   output_mode.nYRes = XN_QVGA_Y_RES;
00885   output_mode.nFPS  = 25;
00886   xn2config_map_[output_mode] = OpenNI_QVGA_25Hz;
00887   config2xn_map_[OpenNI_QVGA_25Hz] = output_mode;
00888   output_mode.nFPS  = 30;
00889   xn2config_map_[output_mode] = OpenNI_QVGA_30Hz;
00890   config2xn_map_[OpenNI_QVGA_30Hz] = output_mode;
00891   output_mode.nFPS  = 60;
00892   xn2config_map_[output_mode] = OpenNI_QVGA_60Hz;
00893   config2xn_map_[OpenNI_QVGA_60Hz] = output_mode;
00894 
00895   output_mode.nXRes = XN_QQVGA_X_RES;
00896   output_mode.nYRes = XN_QQVGA_Y_RES;
00897   output_mode.nFPS  = 25;
00898   xn2config_map_[output_mode] = OpenNI_QQVGA_25Hz;
00899   config2xn_map_[OpenNI_QQVGA_25Hz] = output_mode;
00900   output_mode.nFPS  = 30;
00901   xn2config_map_[output_mode] = OpenNI_QQVGA_30Hz;
00902   config2xn_map_[OpenNI_QQVGA_30Hz] = output_mode;
00903   output_mode.nFPS  = 60;
00904   xn2config_map_[output_mode] = OpenNI_QQVGA_60Hz;
00905   config2xn_map_[OpenNI_QQVGA_60Hz] = output_mode;
00906 }
00907 
00908 int DriverNodelet::mapXnMode2ConfigMode (const XnMapOutputMode& output_mode) const
00909 {
00910   std::map<XnMapOutputMode, int, modeComp>::const_iterator it = xn2config_map_.find (output_mode);
00911 
00912   if (it == xn2config_map_.end ())
00913   {
00914     NODELET_ERROR ("mode %dx%d@%d could not be found", output_mode.nXRes, output_mode.nYRes, output_mode.nFPS);
00915     exit (-1);
00916   }
00917   else
00918     return it->second;
00919 }
00920 
00921 XnMapOutputMode DriverNodelet::mapConfigMode2XnMode (int mode) const
00922 {
00923   std::map<int, XnMapOutputMode>::const_iterator it = config2xn_map_.find (mode);
00924   if (it == config2xn_map_.end ())
00925   {
00926     NODELET_ERROR ("mode %d could not be found", mode);
00927     exit (-1);
00928   }
00929   else
00930     return it->second;
00931 }
00932 
00933 void DriverNodelet::watchDog (const ros::TimerEvent& event)
00934 {
00936   if ( !time_stamp_.isZero() && (device_->isDepthStreamRunning() || device_->isImageStreamRunning()) )
00937   {
00938     ros::Duration duration = ros::Time::now() - time_stamp_;
00939     if (duration.toSec() >= time_out_)
00940     {
00941       NODELET_ERROR("Timeout");
00942       watch_dog_timer_.stop();
00943       throw std::runtime_error("Timeout occured in DriverNodelet");
00944     }
00945   }
00946 }
00947 
00948 }
00949 
00950 // Register as nodelet
00951 #include <pluginlib/class_list_macros.h>
00952 PLUGINLIB_EXPORT_CLASS(openni_camera::DriverNodelet, nodelet::Nodelet)


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Thu Jun 6 2019 20:16:13