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       if (!getPrivateNodeHandle().getParam("device_id", device_id))
00243       {
00244         NODELET_WARN ("~device_id is not set! Using first device.");
00245         device_ = driver.getDeviceByIndex (0);
00246       }
00247       else if (device_id.find ('@') != string::npos)
00248       {
00249         size_t pos = device_id.find ('@');
00250         unsigned bus = atoi (device_id.substr (0, pos).c_str ());
00251         unsigned address = atoi (device_id.substr (pos + 1, device_id.length () - pos - 1).c_str ());
00252         NODELET_INFO ("Searching for device with bus@address = %d@%d", bus, address);
00253         device_ = driver.getDeviceByAddress (bus, address);
00254       }
00255       else if (device_id[0] == '#')
00256       {
00257         unsigned index = atoi (device_id.c_str () + 1);
00258         NODELET_INFO ("Searching for device with index = %d", index);
00259         device_ = driver.getDeviceByIndex (index - 1);
00260       }
00261       else
00262       {
00263         NODELET_INFO ("Searching for device with serial number = '%s'", device_id.c_str ());
00264         device_ = driver.getDeviceBySerialNumber (device_id);
00265       }
00266     }
00267     catch (const OpenNIException& exception)
00268     {
00269       if (!device_)
00270       {
00271         NODELET_INFO ("No matching device found.... waiting for devices. Reason: %s", exception.what ());
00272         boost::this_thread::sleep(boost::posix_time::seconds(3));
00273         continue;
00274       }
00275       else
00276       {
00277         NODELET_ERROR ("Could not retrieve device. Reason: %s", exception.what ());
00278         exit (-1);
00279       }
00280     }
00281   } while (!device_);
00282 
00283   NODELET_INFO ("Opened '%s' on bus %d:%d with serial number '%s'", device_->getProductName (),
00284                 device_->getBus (), device_->getAddress (), device_->getSerialNumber ());
00285 
00286   device_->registerImageCallback(&DriverNodelet::rgbCb,   *this);
00287   device_->registerDepthCallback(&DriverNodelet::depthCb, *this);
00288   device_->registerIRCallback   (&DriverNodelet::irCb,    *this);
00289 }
00290 
00291 void DriverNodelet::rgbConnectCb()
00292 {
00293   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00294   bool need_rgb = pub_rgb_.getNumSubscribers() > 0;
00295   
00296   if (need_rgb && !device_->isImageStreamRunning())
00297   {
00298     // Can't stream IR and RGB at the same time. Give RGB preference.
00299     if (device_->isIRStreamRunning())
00300     {
00301       NODELET_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00302       device_->stopIRStream();
00303     }
00304     
00305     device_->startImageStream();
00306     startSynchronization();
00307     time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
00308   }
00309   else if (!need_rgb && device_->isImageStreamRunning())
00310   {
00311     stopSynchronization();
00312     device_->stopImageStream();
00313 
00314     // Start IR if it's been blocked on RGB subscribers
00315     bool need_ir = pub_ir_.getNumSubscribers() > 0;
00316     if (need_ir && !device_->isIRStreamRunning())
00317     {
00318       device_->startIRStream();
00319       time_stamp_ = ros::Time(0,0);
00320     }
00321   }
00322 }
00323 
00324 void DriverNodelet::depthConnectCb()
00325 {
00326   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00328   bool need_depth =
00329     device_->isDepthRegistered() ? pub_depth_registered_.getNumSubscribers() > 0 : pub_depth_.getNumSubscribers() > 0;
00331 
00332   if (need_depth && !device_->isDepthStreamRunning())
00333   {
00334     device_->startDepthStream();
00335     startSynchronization();
00336     time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
00337   }
00338   else if (!need_depth && device_->isDepthStreamRunning())
00339   {
00340     stopSynchronization();
00341     device_->stopDepthStream();
00342   }
00343 }
00344 
00345 void DriverNodelet::irConnectCb()
00346 {
00347   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00348   bool need_ir = pub_ir_.getNumSubscribers() > 0;
00349   
00350   if (need_ir && !device_->isIRStreamRunning())
00351   {
00352     // Can't stream IR and RGB at the same time
00353     if (device_->isImageStreamRunning())
00354     {
00355       NODELET_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00356     }
00357     else
00358     {
00359       device_->startIRStream();
00360       time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
00361     }
00362   }
00363   else if (!need_ir)
00364   {
00365     device_->stopIRStream();
00366   }
00367 }
00368 
00369 // If any stream is ready, publish next available image from all streams
00370 // This publishes all available data with a maximum time offset of one frame between any two sources
00371 // Need to have lock to call this, since callbacks can be in different threads
00372 void DriverNodelet::checkFrameCounters()
00373 {
00374     if (max(rgb_frame_counter_, max(depth_frame_counter_, ir_frame_counter_)) > config_.data_skip) {
00375         // Reset all counters after we trigger publish
00376         rgb_frame_counter_   = 0;
00377         depth_frame_counter_ = 0;
00378         ir_frame_counter_    = 0;
00379 
00380         // Trigger publish on all topics
00381         publish_rgb_   = true;
00382         publish_depth_ = true;
00383         publish_ir_    = true;
00384     }
00385 }
00386 
00387 void DriverNodelet::rgbCb(boost::shared_ptr<openni_wrapper::Image> image, void* cookie)
00388 {
00389   if (!config_init_)
00390     return;
00391 
00392   ros::Time time = ros::Time::now () + ros::Duration(config_.image_time_offset);
00393   time_stamp_ = time; // for watchdog
00394 
00395   bool publish = false;
00396   {
00397       boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00398       rgb_frame_counter_++;
00399       checkFrameCounters();
00400       publish = publish_rgb_;
00401 
00402       if (publish)
00403           rgb_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
00404   }
00405 
00406   if (publish)
00407       publishRgbImage(*image, time);
00408 
00409   publish_rgb_ = false;
00410 }
00411 
00412 void DriverNodelet::depthCb(boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void* cookie)
00413 {
00414   if (!config_init_)
00415     return;
00416 
00417   ros::Time time = ros::Time::now () + ros::Duration(config_.depth_time_offset);
00418   time_stamp_ = time; // for watchdog
00419 
00420   bool publish = false;
00421   {
00422       boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00423       depth_frame_counter_++;
00424       checkFrameCounters();
00425       publish = publish_depth_;
00426 
00427       if (publish)
00428           depth_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
00429   }
00430 
00431   if (publish)
00432       publishDepthImage(*depth_image, time);
00433 
00434   publish_depth_ = false;
00435 }
00436 
00437 void DriverNodelet::irCb(boost::shared_ptr<openni_wrapper::IRImage> ir_image, void* cookie)
00438 {
00439   if (!config_init_)
00440     return;
00441 
00442   ros::Time time = ros::Time::now() + ros::Duration(config_.depth_time_offset);
00443   time_stamp_ = time; // for watchdog
00444 
00445   bool publish = false;
00446   {
00447       boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00448       ir_frame_counter_++;
00449       checkFrameCounters();
00450       publish = publish_ir_;
00451 
00452       if (publish)
00453           ir_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
00454   }
00455 
00456   if (publish)
00457       publishIrImage(*ir_image, time);
00458   publish_ir_ = false;
00459 }
00460 
00461 void DriverNodelet::publishRgbImage(const openni_wrapper::Image& image, ros::Time time) const
00462 {
00463   sensor_msgs::ImagePtr rgb_msg = boost::make_shared<sensor_msgs::Image >();
00464   rgb_msg->header.stamp = time;
00465   rgb_msg->header.frame_id = rgb_frame_id_;
00466   bool downscale = false;
00467   if (image.getEncoding() == openni_wrapper::Image::BAYER_GRBG)
00468   {
00469     if (image.getWidth() == image_width_ && image.getHeight() == image_height_)
00470     {
00471       // image sizes match, we can copy directly
00472       rgb_msg->encoding = sensor_msgs::image_encodings::BAYER_GRBG8;
00473       rgb_msg->step = image_width_;
00474     }
00475     else
00476     {
00477       // image sizes missmatch, we have to downscale and de-bayer in this function
00478       rgb_msg->encoding = sensor_msgs::image_encodings::RGB8;
00479       rgb_msg->step = image_width_ * 3;
00480       downscale = true;
00481     }
00482   }
00483   else if (image.getEncoding() == openni_wrapper::Image::YUV422)
00484   {
00485     if (image.getWidth() == image_width_ && image.getHeight() == image_height_)
00486     {
00487       // image sizes match, we can copy directly
00488       rgb_msg->encoding = sensor_msgs::image_encodings::YUV422;
00489       rgb_msg->step = image_width_ * 2; // 4 bytes for 2 pixels
00490     }
00491     else
00492     {
00493       // image sizes missmatch, we have to downscale and convert in this function
00494       rgb_msg->encoding = sensor_msgs::image_encodings::RGB8;
00495       rgb_msg->step = image_width_ * 3;
00496       downscale = true;
00497     }
00498   }
00499   rgb_msg->height = image_height_;
00500   rgb_msg->width = image_width_;
00501   rgb_msg->data.resize(rgb_msg->height * rgb_msg->step);
00502   
00503   try
00504   {
00505     if (downscale)
00506     {
00507       if (image.getEncoding() == openni_wrapper::Image::BAYER_GRBG)
00508       {
00509         openni_wrapper::ImageBayerGRBG bayer_image(image.getMetaDataPtr(), openni_wrapper::ImageBayerGRBG::Bilinear);
00510         bayer_image.fillRGB(image_width_, image_height_, &rgb_msg->data[0]);
00511       }
00512       else if (image.getEncoding() == openni_wrapper::Image::YUV422)
00513       {
00514         openni_wrapper::ImageYUV422 yuv_image(image.getMetaDataPtr());
00515         yuv_image.fillRGB(image_width_, image_height_, &rgb_msg->data[0]);
00516       }
00517     }
00518     else
00519       image.fillRaw(&rgb_msg->data[0]);
00520   }
00521   catch ( OpenNIException e )
00522   {
00523     ROS_ERROR_THROTTLE(1,e.what());
00524   }
00525   
00526   pub_rgb_.publish(rgb_msg, getRgbCameraInfo(rgb_msg->width,rgb_msg->height,time));
00527 }
00528 
00529 void DriverNodelet::publishDepthImage(const openni_wrapper::DepthImage& depth, ros::Time time) const
00530 {
00531   bool registered = device_->isDepthRegistered();
00532   
00534   sensor_msgs::ImagePtr depth_msg = boost::make_shared<sensor_msgs::Image>();
00535   depth_msg->header.stamp    = time;
00536   depth_msg->encoding        = sensor_msgs::image_encodings::TYPE_16UC1;
00537   depth_msg->width           = depth_width_;
00538   depth_msg->height          = depth_height_;
00539   depth_msg->step            = depth_msg->width * sizeof(short);
00540   depth_msg->data.resize(depth_msg->height * depth_msg->step);
00541 
00542   try
00543   {
00544     depth.fillDepthImageRaw(depth_msg->width, depth_msg->height, reinterpret_cast<unsigned short*>(&depth_msg->data[0]),
00545                             depth_msg->step);
00546   }
00547   catch ( OpenNIException e )
00548   {
00549     ROS_ERROR_THROTTLE(1,e.what());
00550   }
00551 
00552   if (fabs(z_scaling_ - 1.0) > 1e-6)
00553   {
00554     uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg->data[0]);
00555     for (unsigned int i = 0; i < depth_msg->width * depth_msg->height; ++i)
00556       if (data[i] != 0)
00557             data[i] = static_cast<uint16_t>(data[i] * z_scaling_);
00558   }
00559 
00560   if (z_offset_mm_ != 0)
00561   {
00562     uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg->data[0]);
00563     for (unsigned int i = 0; i < depth_msg->width * depth_msg->height; ++i)
00564       if (data[i] != 0)
00565             data[i] += z_offset_mm_;
00566   }
00567 
00568   if (registered)
00569   {
00570     // Publish RGB camera info and raw depth image to depth_registered/ ns
00571     depth_msg->header.frame_id = rgb_frame_id_;
00572     pub_depth_registered_.publish(depth_msg, getRgbCameraInfo(depth_msg->width, depth_msg->height, time));
00573   }
00574   else
00575   {
00576     // Publish depth camera info and raw depth image to depth/ ns
00577     depth_msg->header.frame_id = depth_frame_id_;
00578     pub_depth_.publish(depth_msg, getDepthCameraInfo(depth_msg->width, depth_msg->height, time));
00579   }
00580 
00581   // Projector "info" probably only useful for working with disparity images
00582   if (pub_projector_info_.getNumSubscribers() > 0)
00583   {
00584     pub_projector_info_.publish(getProjectorCameraInfo(depth_msg->width, depth_msg->height, time));
00585   }
00586 }
00587 
00588 void DriverNodelet::publishIrImage(const openni_wrapper::IRImage& ir, ros::Time time) const
00589 {
00590   sensor_msgs::ImagePtr ir_msg = boost::make_shared<sensor_msgs::Image>();
00591   ir_msg->header.stamp    = time;
00592   ir_msg->header.frame_id = depth_frame_id_;
00593   ir_msg->encoding        = sensor_msgs::image_encodings::MONO16;
00594   ir_msg->height          = ir.getHeight();
00595   ir_msg->width           = ir.getWidth();
00596   ir_msg->step            = ir_msg->width * sizeof(uint16_t);
00597   ir_msg->data.resize(ir_msg->height * ir_msg->step);
00598 
00599   try
00600   {
00601     ir.fillRaw(ir.getWidth(), ir.getHeight(), reinterpret_cast<unsigned short*>(&ir_msg->data[0]));
00602   }
00603   catch ( OpenNIException e )
00604   {
00605     ROS_ERROR_THROTTLE(1,e.what());
00606   }
00607 
00608   pub_ir_.publish(ir_msg, getIrCameraInfo(ir.getWidth(), ir.getHeight(), time));
00609 }
00610 
00611 sensor_msgs::CameraInfoPtr DriverNodelet::getDefaultCameraInfo(int width, int height, double f) const
00612 {
00613   sensor_msgs::CameraInfoPtr info = boost::make_shared<sensor_msgs::CameraInfo>();
00614 
00615   info->width  = width;
00616   info->height = height;
00617 
00618   // No distortion
00619   info->D.resize(5, 0.0);
00620   info->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00621 
00622   // Simple camera matrix: square pixels (fx = fy), principal point at center
00623   info->K.assign(0.0);
00624   info->K[0] = info->K[4] = f;
00625   info->K[2] = (width / 2) - 0.5;
00626   // Aspect ratio for the camera center on Kinect (and other devices?) is 4/3
00627   // This formula keeps the principal point the same in VGA and SXGA modes
00628   info->K[5] = (width * (3./8.)) - 0.5;
00629   info->K[8] = 1.0;
00630 
00631   // No separate rectified image plane, so R = I
00632   info->R.assign(0.0);
00633   info->R[0] = info->R[4] = info->R[8] = 1.0;
00634 
00635   // Then P=K(I|0) = (K|0)
00636   info->P.assign(0.0);
00637   info->P[0]  = info->P[5] = f; // fx, fy
00638   info->P[2]  = info->K[2];     // cx
00639   info->P[6]  = info->K[5];     // cy
00640   info->P[10] = 1.0;
00641 
00642   return info;
00643 }
00644 
00646 sensor_msgs::CameraInfoPtr DriverNodelet::getRgbCameraInfo(int width, int height, ros::Time time) const
00647 {
00648   sensor_msgs::CameraInfoPtr info;
00649 
00650   if (rgb_info_manager_->isCalibrated())
00651   {
00652     info = boost::make_shared<sensor_msgs::CameraInfo>(rgb_info_manager_->getCameraInfo());
00653     if ( info->width != width )
00654     {
00655       // Use uncalibrated values
00656       ROS_WARN_ONCE("Image resolution doesn't match calibration of the RGB camera. Using default parameters.");
00657       info = getDefaultCameraInfo(width, height, device_->getImageFocalLength(width));
00658     }
00659   }
00660   else
00661   {
00662     // If uncalibrated, fill in default values
00663     info = getDefaultCameraInfo(width, height, device_->getImageFocalLength(width));
00664   }
00665 
00666   // Fill in header
00667   info->header.stamp    = time;
00668   info->header.frame_id = rgb_frame_id_;
00669 
00670   return info;
00671 }
00672 
00673 sensor_msgs::CameraInfoPtr DriverNodelet::getIrCameraInfo(int width, int height, ros::Time time) const
00674 {
00675   sensor_msgs::CameraInfoPtr info;
00676 
00677   if (ir_info_manager_->isCalibrated())
00678   {
00679     info = boost::make_shared<sensor_msgs::CameraInfo>(ir_info_manager_->getCameraInfo());
00680     if ( info->width != width )
00681     {
00682       // Use uncalibrated values
00683       ROS_WARN_ONCE("Image resolution doesn't match calibration of the IR camera. Using default parameters.");
00684       info = getDefaultCameraInfo(width, height, device_->getImageFocalLength(width));
00685     }
00686   }
00687   else
00688   {
00689     // If uncalibrated, fill in default values
00690     info = getDefaultCameraInfo(width, height, device_->getDepthFocalLength(width));
00691   }
00692 
00693   // Fill in header
00694   info->header.stamp    = time;
00695   info->header.frame_id = depth_frame_id_;
00696 
00697   return info;
00698 }
00699 
00700 sensor_msgs::CameraInfoPtr DriverNodelet::getDepthCameraInfo(int width, int height, ros::Time time) const
00701 {
00702   // The depth image has essentially the same intrinsics as the IR image, BUT the
00703   // principal point is offset by half the size of the hardware correlation window
00704   // (probably 9x9 or 9x7 in 640x480 mode). See http://www.ros.org/wiki/kinect_calibration/technical
00705 
00706   double scaling = (double)width / 640;
00707 
00708   sensor_msgs::CameraInfoPtr info = getIrCameraInfo(width, height, time);
00709   info->K[2] -= depth_ir_offset_x_*scaling; // cx
00710   info->K[5] -= depth_ir_offset_y_*scaling; // cy
00711   info->P[2] -= depth_ir_offset_x_*scaling; // cx
00712   info->P[6] -= depth_ir_offset_y_*scaling; // cy
00713 
00715   return info;
00716 }
00717 
00718 sensor_msgs::CameraInfoPtr DriverNodelet::getProjectorCameraInfo(int width, int height, ros::Time time) const
00719 {
00720   // The projector info is simply the depth info with the baseline encoded in the P matrix.
00721   // It's only purpose is to be the "right" camera info to the depth camera's "left" for
00722   // processing disparity images.
00723   sensor_msgs::CameraInfoPtr info = getDepthCameraInfo(width, height, time);
00724   // Tx = -baseline * fx
00725   info->P[3] = -device_->getBaseline() * info->P[0];
00726   return info;
00727 }
00728 
00729 void DriverNodelet::configCb(Config &config, uint32_t level)
00730 {
00731   depth_ir_offset_x_ = config.depth_ir_offset_x;
00732   depth_ir_offset_y_ = config.depth_ir_offset_y;
00733   z_offset_mm_ = config.z_offset_mm;
00734   z_scaling_ = config.z_scaling;
00735 
00736   // Based on the config options, try to set the depth/image mode
00737   // The device driver might decide to switch to a 'compatible mode',
00738   // i.e. a higher resolution than the requested one, in which case it
00739   // will later downsample the image.
00740   // Only if the 'compatible mode' is different from the current one, we will
00741   // need to reset the streams.
00742 
00743   bool depth_mode_changed = false;
00744   bool image_mode_changed = false;
00745 
00746   XnMapOutputMode compatible_image_mode;
00747   XnMapOutputMode compatible_depth_mode;
00748 
00749   if (device_->hasImageStream ())
00750   {
00751     XnMapOutputMode image_mode = mapConfigMode2XnMode (config.image_mode);
00752     image_width_  = image_mode.nXRes;
00753     image_height_ = image_mode.nYRes;
00754 
00755     // does the device support the new image mode?
00756     if (!device_->findCompatibleImageMode (image_mode, compatible_image_mode))
00757     {
00758       XnMapOutputMode default_mode = device_->getDefaultImageMode();
00759       NODELET_WARN("Could not find any compatible image output mode for %d x %d @ %d. "
00760                    "Falling back to default image output mode %d x %d @ %d.",
00761                     image_mode.nXRes, image_mode.nYRes, image_mode.nFPS,
00762                     default_mode.nXRes, default_mode.nYRes, default_mode.nFPS);
00763 
00764       config.image_mode = mapXnMode2ConfigMode(default_mode);
00765       image_mode = compatible_image_mode = default_mode;
00766     }
00767 
00768     if ( compatible_image_mode != device_->getImageOutputMode () )
00769     {
00770       image_mode_changed = true;
00771     }
00772   }
00773   
00774   if (device_->hasDepthStream())
00775   {
00776     XnMapOutputMode depth_mode = mapConfigMode2XnMode (config.depth_mode);
00777     depth_width_  = depth_mode.nXRes;
00778     depth_height_ = depth_mode.nYRes;
00779 
00780     // does the device support the new depth mode?
00781     if (!device_->findCompatibleDepthMode (depth_mode, compatible_depth_mode))
00782     {
00783       XnMapOutputMode default_mode = device_->getDefaultDepthMode();
00784       NODELET_WARN ("Could not find any compatible depth output mode for %d x %d @ %d. "
00785                     "Falling back to default depth output mode %d x %d @ %d.",
00786                     depth_mode.nXRes, depth_mode.nYRes, depth_mode.nFPS,
00787                     default_mode.nXRes, default_mode.nYRes, default_mode.nFPS);
00788 
00789       config.depth_mode = mapXnMode2ConfigMode(default_mode);
00790       depth_mode = compatible_depth_mode = default_mode;
00791     }
00792 
00793     if ( compatible_depth_mode != device_->getDepthOutputMode () )
00794     {
00795       depth_mode_changed = true;
00796     }
00797   }
00798 
00799   // here everything is fine. Now make the changes
00800   if ( image_mode_changed || depth_mode_changed )
00801   {
00802     // streams need to be reset!
00803     stopSynchronization();
00804 
00805     if ( image_mode_changed )
00806     {
00807       device_->setImageOutputMode (compatible_image_mode);
00808     }
00809 
00810     if ( depth_mode_changed )
00811     {
00812       device_->setDepthOutputMode (compatible_depth_mode);
00813     }
00814 
00815     startSynchronization ();
00816   }
00817 
00819   if (device_->isDepthRegistered () && !config.depth_registration)
00820   {
00821     device_->setDepthRegistration (false);
00822   }
00823   else if (!device_->isDepthRegistered () && config.depth_registration)
00824   {
00825     device_->setDepthRegistration (true);
00826   }
00827 
00828   // now we can copy
00829   config_ = config;
00830 
00831   config_init_ = true;
00832 }
00833 
00834 void DriverNodelet::startSynchronization()
00835 {
00836   if (device_->isSynchronizationSupported() &&
00837       !device_->isSynchronized() &&
00838       device_->getImageOutputMode().nFPS == device_->getDepthOutputMode().nFPS &&
00839       device_->isImageStreamRunning() &&
00840       device_->isDepthStreamRunning() )
00841   {
00842     device_->setSynchronization(true);
00843   }
00844 }
00845 
00846 void DriverNodelet::stopSynchronization()
00847 {
00848   if (device_->isSynchronizationSupported() &&
00849       device_->isSynchronized())
00850   {
00851     device_->setSynchronization(false);
00852   }
00853 }
00854 
00855 void DriverNodelet::updateModeMaps ()
00856 {
00857   XnMapOutputMode output_mode;
00858 
00859   output_mode.nXRes = XN_SXGA_X_RES;
00860   output_mode.nYRes = XN_SXGA_Y_RES;
00861   output_mode.nFPS  = 15;
00862   xn2config_map_[output_mode] = OpenNI_SXGA_15Hz;
00863   config2xn_map_[OpenNI_SXGA_15Hz] = output_mode;
00864 
00865   output_mode.nXRes = XN_VGA_X_RES;
00866   output_mode.nYRes = XN_VGA_Y_RES;
00867   output_mode.nFPS  = 25;
00868   xn2config_map_[output_mode] = OpenNI_VGA_25Hz;
00869   config2xn_map_[OpenNI_VGA_25Hz] = output_mode;
00870   output_mode.nFPS  = 30;
00871   xn2config_map_[output_mode] = OpenNI_VGA_30Hz;
00872   config2xn_map_[OpenNI_VGA_30Hz] = output_mode;
00873 
00874   output_mode.nXRes = XN_QVGA_X_RES;
00875   output_mode.nYRes = XN_QVGA_Y_RES;
00876   output_mode.nFPS  = 25;
00877   xn2config_map_[output_mode] = OpenNI_QVGA_25Hz;
00878   config2xn_map_[OpenNI_QVGA_25Hz] = output_mode;
00879   output_mode.nFPS  = 30;
00880   xn2config_map_[output_mode] = OpenNI_QVGA_30Hz;
00881   config2xn_map_[OpenNI_QVGA_30Hz] = output_mode;
00882   output_mode.nFPS  = 60;
00883   xn2config_map_[output_mode] = OpenNI_QVGA_60Hz;
00884   config2xn_map_[OpenNI_QVGA_60Hz] = output_mode;
00885 
00886   output_mode.nXRes = XN_QQVGA_X_RES;
00887   output_mode.nYRes = XN_QQVGA_Y_RES;
00888   output_mode.nFPS  = 25;
00889   xn2config_map_[output_mode] = OpenNI_QQVGA_25Hz;
00890   config2xn_map_[OpenNI_QQVGA_25Hz] = output_mode;
00891   output_mode.nFPS  = 30;
00892   xn2config_map_[output_mode] = OpenNI_QQVGA_30Hz;
00893   config2xn_map_[OpenNI_QQVGA_30Hz] = output_mode;
00894   output_mode.nFPS  = 60;
00895   xn2config_map_[output_mode] = OpenNI_QQVGA_60Hz;
00896   config2xn_map_[OpenNI_QQVGA_60Hz] = output_mode;
00897 }
00898 
00899 int DriverNodelet::mapXnMode2ConfigMode (const XnMapOutputMode& output_mode) const
00900 {
00901   std::map<XnMapOutputMode, int, modeComp>::const_iterator it = xn2config_map_.find (output_mode);
00902 
00903   if (it == xn2config_map_.end ())
00904   {
00905     NODELET_ERROR ("mode %dx%d@%d could not be found", output_mode.nXRes, output_mode.nYRes, output_mode.nFPS);
00906     exit (-1);
00907   }
00908   else
00909     return it->second;
00910 }
00911 
00912 XnMapOutputMode DriverNodelet::mapConfigMode2XnMode (int mode) const
00913 {
00914   std::map<int, XnMapOutputMode>::const_iterator it = config2xn_map_.find (mode);
00915   if (it == config2xn_map_.end ())
00916   {
00917     NODELET_ERROR ("mode %d could not be found", mode);
00918     exit (-1);
00919   }
00920   else
00921     return it->second;
00922 }
00923 
00924 void DriverNodelet::watchDog (const ros::TimerEvent& event)
00925 {
00927   if ( !time_stamp_.isZero() && (device_->isDepthStreamRunning() || device_->isImageStreamRunning()) )
00928   {
00929     ros::Duration duration = ros::Time::now() - time_stamp_;
00930     if (duration.toSec() >= time_out_)
00931     {
00932       NODELET_ERROR("Timeout");
00933       watch_dog_timer_.stop();
00934       throw std::runtime_error("Timeout occured in DriverNodelet");
00935     }
00936   }
00937 }
00938 
00939 }
00940 
00941 // Register as nodelet
00942 #include <pluginlib/class_list_macros.h>
00943 PLUGINLIB_DECLARE_CLASS (openni_camera, driver, openni_camera::DriverNodelet, nodelet::Nodelet);


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Wed Aug 26 2015 15:08:49