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 <sensor_msgs/image_encodings.h>
00041 #include <sensor_msgs/distortion_models.h>
00042 #include <boost/algorithm/string/replace.hpp>
00043 #include <log4cxx/logger.h>
00044 
00045 using namespace std;
00046 namespace freenect_camera {
00047 
00048 DriverNodelet::~DriverNodelet ()
00049 {
00050   // If we're still stuck in initialization (e.g. can't connect to device), break out
00051   init_thread_.interrupt();
00052   init_thread_.join();
00053 
00054   // Interrupt and close diagnostics thread
00055   close_diagnostics_ = true;
00056   diagnostics_thread_.join();
00057 
00058   FreenectDriver& driver = FreenectDriver::getInstance ();
00059   driver.shutdown();
00060 
00063 }
00064 
00065 void DriverNodelet::onInit ()
00066 {
00067   // Setting up device can take awhile but onInit shouldn't block, so we spawn a
00068   // new thread to do all the initialization
00069   init_thread_ = boost::thread(boost::bind(&DriverNodelet::onInitImpl, this));
00070 }
00071 
00072 void DriverNodelet::onInitImpl ()
00073 {
00074   ros::NodeHandle& nh       = getNodeHandle();        // topics
00075   ros::NodeHandle& param_nh = getPrivateNodeHandle(); // parameters
00076 
00077   // Allow remapping namespaces rgb, ir, depth, depth_registered
00078   image_transport::ImageTransport it(nh);
00079   ros::NodeHandle rgb_nh(nh, "rgb");
00080   image_transport::ImageTransport rgb_it(rgb_nh);
00081   ros::NodeHandle ir_nh(nh, "ir");
00082   image_transport::ImageTransport ir_it(ir_nh);
00083   ros::NodeHandle depth_nh(nh, "depth");
00084   image_transport::ImageTransport depth_it(depth_nh);
00085   ros::NodeHandle depth_registered_nh(nh, "depth_registered");
00086   image_transport::ImageTransport depth_registered_it(depth_registered_nh);
00087   ros::NodeHandle projector_nh(nh, "projector");
00088 
00089   rgb_frame_counter_ = depth_frame_counter_ = ir_frame_counter_ = 0;
00090   publish_rgb_ = publish_ir_ = publish_depth_ = true;
00091 
00092   // Check to see if we should enable debugging messages in libfreenect
00093   // libfreenect_debug_ should be set before calling setupDevice
00094   param_nh.param("debug" , libfreenect_debug_, false);
00095 
00096   // Initialize the sensor, but don't start any streams yet. That happens in the connection callbacks.
00097   updateModeMaps();
00098   setupDevice();
00099 
00100   // Initialize dynamic reconfigure
00101   reconfigure_server_.reset( new ReconfigureServer(param_nh) );
00102   reconfigure_server_->setCallback(boost::bind(&DriverNodelet::configCb, this, _1, _2));
00103 
00104   // Camera TF frames
00105   param_nh.param("rgb_frame_id",   rgb_frame_id_,   string("/openni_rgb_optical_frame"));
00106   param_nh.param("depth_frame_id", depth_frame_id_, string("/openni_depth_optical_frame"));
00107   NODELET_INFO("rgb_frame_id = '%s' ",   rgb_frame_id_.c_str());
00108   NODELET_INFO("depth_frame_id = '%s' ", depth_frame_id_.c_str());
00109 
00110   // Pixel offset between depth and IR images.
00111   // By default assume offset of (5,4) from 9x7 correlation window.
00112   // NOTE: These are now (temporarily?) dynamically reconfigurable, to allow tweaking.
00113   //param_nh.param("depth_ir_offset_x", depth_ir_offset_x_, 5.0);
00114   //param_nh.param("depth_ir_offset_y", depth_ir_offset_y_, 4.0);
00115 
00116   // The camera names are set to [rgb|depth]_[serial#], e.g. depth_B00367707227042B.
00117   // camera_info_manager substitutes this for ${NAME} in the URL.
00118   std::string serial_number = device_->getSerialNumber();
00119   std::string rgb_name, ir_name;
00120   if (serial_number.empty())
00121   {
00122     rgb_name = "rgb";
00123     ir_name  = "depth"; 
00124   }
00125   else
00126   {
00127     rgb_name = "rgb_"   + serial_number;
00128     ir_name  = "depth_" + serial_number;
00129   }
00130 
00131   std::string rgb_info_url, ir_info_url;
00132   param_nh.param("rgb_camera_info_url", rgb_info_url, std::string());
00133   param_nh.param("depth_camera_info_url", ir_info_url, std::string());
00134 
00135   double diagnostics_max_frequency, diagnostics_min_frequency;
00136   double diagnostics_tolerance, diagnostics_window_time;
00137   param_nh.param("enable_rgb_diagnostics", enable_rgb_diagnostics_, false);
00138   param_nh.param("enable_ir_diagnostics", enable_ir_diagnostics_, false);
00139   param_nh.param("enable_depth_diagnostics", enable_depth_diagnostics_, false);
00140   param_nh.param("diagnostics_max_frequency", diagnostics_max_frequency, 30.0);
00141   param_nh.param("diagnostics_min_frequency", diagnostics_min_frequency, 30.0);
00142   param_nh.param("diagnostics_tolerance", diagnostics_tolerance, 0.05);
00143   param_nh.param("diagnostics_window_time", diagnostics_window_time, 5.0);
00144 
00145   // Suppress some of the output from loading camera calibrations (kinda hacky)
00146   log4cxx::LoggerPtr logger_ccp = log4cxx::Logger::getLogger("ros.camera_calibration_parsers");
00147   log4cxx::LoggerPtr logger_cim = log4cxx::Logger::getLogger("ros.camera_info_manager");
00148   logger_ccp->setLevel(log4cxx::Level::getFatal());
00149   logger_cim->setLevel(log4cxx::Level::getWarn());
00150   // Also suppress sync warnings from image_transport::CameraSubscriber. When subscribing to
00151   // depth_registered/foo with Freenect registration disabled, the rectify nodelet for depth_registered/
00152   // will complain because it receives depth_registered/camera_info (from the register nodelet), but
00153   // the driver is not publishing depth_registered/image_raw.
00154   log4cxx::LoggerPtr logger_its = log4cxx::Logger::getLogger("ros.image_transport.sync");
00155   logger_its->setLevel(log4cxx::Level::getError());
00156   ros::console::notifyLoggerLevelsChanged();
00157   
00158   // Load the saved calibrations, if they exist
00159   rgb_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(rgb_nh, rgb_name, rgb_info_url);
00160   ir_info_manager_  = boost::make_shared<camera_info_manager::CameraInfoManager>(ir_nh,  ir_name,  ir_info_url);
00161 
00162   if (!rgb_info_manager_->isCalibrated())
00163     NODELET_WARN("Using default parameters for RGB camera calibration.");
00164   if (!ir_info_manager_->isCalibrated())
00165     NODELET_WARN("Using default parameters for IR camera calibration.");
00166 
00167   // Advertise all published topics
00168   {
00169     // Prevent connection callbacks from executing until we've set all the publishers. Otherwise
00170     // connectCb() can fire while we're advertising (say) "depth/image_raw", but before we actually
00171     // assign to pub_depth_. Then pub_depth_.getNumSubscribers() returns 0, and we fail to start
00172     // the depth generator.
00173     boost::lock_guard<boost::mutex> lock(connect_mutex_);
00174 
00175     // Instantiate the diagnostic updater
00176     pub_freq_max_ = diagnostics_max_frequency;
00177     pub_freq_min_ = diagnostics_min_frequency;
00178     diagnostic_updater_.reset(new diagnostic_updater::Updater);
00179 
00180     // Set hardware id
00181     std::string hardware_id = std::string(device_->getProductName()) + "-" +
00182         std::string(device_->getSerialNumber());
00183     diagnostic_updater_->setHardwareID(hardware_id);
00184     
00185     // Asus Xtion PRO does not have an RGB camera
00186     if (device_->hasImageStream())
00187     {
00188       image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
00189       ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
00190       pub_rgb_ = rgb_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00191       if (enable_rgb_diagnostics_) {
00192         pub_rgb_freq_.reset(new TopicDiagnostic("RGB Image", *diagnostic_updater_,
00193             FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_, 
00194                 diagnostics_tolerance, diagnostics_window_time)));
00195       }
00196     }
00197 
00198     if (device_->hasIRStream())
00199     {
00200       image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::irConnectCb, this);
00201       ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::irConnectCb, this);
00202       pub_ir_ = ir_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00203       if (enable_ir_diagnostics_) {
00204         pub_ir_freq_.reset(new TopicDiagnostic("IR Image", *diagnostic_updater_,
00205             FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_, 
00206                 diagnostics_tolerance, diagnostics_window_time)));
00207       }
00208     }
00209 
00210     if (device_->hasDepthStream())
00211     {
00212       image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::depthConnectCb, this);
00213       ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::depthConnectCb, this);
00214       pub_depth_ = depth_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00215       if (enable_depth_diagnostics_) {
00216         pub_depth_freq_.reset(new TopicDiagnostic("Depth Image", *diagnostic_updater_,
00217             FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_, 
00218                 diagnostics_tolerance, diagnostics_window_time)));
00219       }
00220 
00221       pub_projector_info_ = projector_nh.advertise<sensor_msgs::CameraInfo>("camera_info", 1, rssc, rssc);
00222       
00223       if (device_->isDepthRegistrationSupported()) {
00224         pub_depth_registered_ = depth_registered_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
00225       }
00226     }
00227   }
00228 
00229   // Create separate diagnostics thread
00230   close_diagnostics_ = false;
00231   diagnostics_thread_ = boost::thread(boost::bind(&DriverNodelet::updateDiagnostics, this));
00232 
00233   // Create watch dog timer callback
00234   param_nh.param<double>("time_out", time_out_, 5.0);
00235   if (time_out_ > 0.0)
00236   {
00237     watch_dog_timer_ = nh.createTimer(ros::Duration(time_out_), &DriverNodelet::watchDog, this);
00238   }
00239 
00240   device_->publishersAreReady();
00241 }
00242 
00243 void DriverNodelet::updateDiagnostics() {
00244   while (!close_diagnostics_) {
00245     diagnostic_updater_->update();
00246     boost::this_thread::sleep(boost::posix_time::milliseconds(10));
00247   }
00248 }
00249 
00250 void DriverNodelet::setupDevice ()
00251 {
00252   // Initialize the openni device
00253   FreenectDriver& driver = FreenectDriver::getInstance();
00254 
00255   // Enable debugging in libfreenect if requested
00256   if (libfreenect_debug_)
00257     driver.enableDebug();
00258 
00259   do {
00260     driver.updateDeviceList ();
00261 
00262     if (driver.getNumberDevices () == 0)
00263     {
00264       NODELET_INFO ("No devices connected.... waiting for devices to be connected");
00265       boost::this_thread::sleep(boost::posix_time::seconds(3));
00266       continue;
00267     }
00268 
00269     NODELET_INFO ("Number devices connected: %d", driver.getNumberDevices ());
00270     for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00271     {
00272       NODELET_INFO("%u. device on bus %03u:%02u is a %s (%03x) from %s (%03x) with serial id \'%s\'",
00273                    deviceIdx + 1, driver.getBus(deviceIdx), driver.getAddress(deviceIdx),
00274                    driver.getProductName(deviceIdx), driver.getProductID(deviceIdx),
00275                    driver.getVendorName(deviceIdx), driver.getVendorID(deviceIdx),
00276                    driver.getSerialNumber(deviceIdx));
00277     }
00278 
00279     try {
00280       string device_id;
00281       if (!getPrivateNodeHandle().getParam("device_id", device_id))
00282       {
00283         NODELET_WARN ("~device_id is not set! Using first device.");
00284         device_ = driver.getDeviceByIndex(0);
00285       }
00286       else if (device_id.find ('@') != string::npos)
00287       {
00288         size_t pos = device_id.find ('@');
00289         unsigned bus = atoi (device_id.substr (0, pos).c_str ());
00290         unsigned address = atoi (device_id.substr (pos + 1, device_id.length () - pos - 1).c_str ());
00291         NODELET_INFO ("Searching for device with bus@address = %d@%d", bus, address);
00292         device_ = driver.getDeviceByAddress (bus, address);
00293       }
00294       else if (device_id[0] == '#')
00295       {
00296         unsigned index = atoi (device_id.c_str() + 1);
00297         NODELET_INFO ("Searching for device with index = %d", index);
00298         device_ = driver.getDeviceByIndex (index - 1);
00299       }
00300       else
00301       {
00302         NODELET_INFO ("Searching for device with serial number = '%s'", device_id.c_str ());
00303         device_ = driver.getDeviceBySerialNumber (device_id);
00304       }
00305     }
00306     catch (exception& e)
00307     {
00308       if (!device_)
00309       {
00310         NODELET_INFO ("No matching device found.... waiting for devices. Reason: %s", e.what ());
00311         boost::this_thread::sleep(boost::posix_time::seconds(3));
00312         continue;
00313       }
00314       else
00315       {
00316         NODELET_FATAL ("Could not retrieve device. Reason: %s", e.what ());
00317         exit (-1);
00318       }
00319     }
00320   } while (!device_);
00321 
00322   NODELET_INFO ("Opened '%s' on bus %d:%d with serial number '%s'", device_->getProductName (),
00323                 device_->getBus (), device_->getAddress (), device_->getSerialNumber ());
00324 
00325   device_->registerImageCallback(&DriverNodelet::rgbCb,   *this);
00326   device_->registerDepthCallback(&DriverNodelet::depthCb, *this);
00327   device_->registerIRCallback   (&DriverNodelet::irCb,    *this);
00328 }
00329 
00330 void DriverNodelet::rgbConnectCb()
00331 {
00332   //std::cout << "rgb connect cb called";
00333   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00334   //std::cout << "..." << std::endl;
00335   bool need_rgb = pub_rgb_.getNumSubscribers() > 0;
00336   //std::cout << "  need_rgb: " << need_rgb << std::endl;
00337   
00338   if (need_rgb && !device_->isImageStreamRunning())
00339   {
00340     // Can't stream IR and RGB at the same time. Give RGB preference.
00341     if (device_->isIRStreamRunning())
00342     {
00343       NODELET_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00344       device_->stopIRStream();
00345     }
00346     
00347     device_->startImageStream();
00348     startSynchronization();
00349     rgb_time_stamp_ = ros::Time::now(); // update stamp for watchdog 
00350   }
00351   else if (!need_rgb && device_->isImageStreamRunning())
00352   {
00353     stopSynchronization();
00354     device_->stopImageStream();
00355 
00356     // Start IR if it's been blocked on RGB subscribers
00357     bool need_ir = pub_ir_.getNumSubscribers() > 0;
00358     if (need_ir && !device_->isIRStreamRunning())
00359     {
00360       device_->startIRStream();
00361       ir_time_stamp_ = ros::Time::now(); // update stamp for watchdog
00362     }
00363   }
00364   //std::cout << "rgb connect cb end..." << std::endl;
00365 }
00366 
00367 void DriverNodelet::depthConnectCb()
00368 {
00369   //std::cout << "depth connect cb called";
00370   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00371   //std::cout << "..." << std::endl;
00373   bool need_depth =
00374     device_->isDepthRegistered() ? pub_depth_registered_.getNumSubscribers() > 0 : pub_depth_.getNumSubscribers() > 0;
00376   //std::cout << "  need_depth: " << need_depth << std::endl;
00377 
00378   if (need_depth && !device_->isDepthStreamRunning())
00379   {
00380     device_->startDepthStream();
00381     startSynchronization();
00382     depth_time_stamp_ = ros::Time::now(); // update stamp for watchdog
00383 
00384   }
00385   else if (!need_depth && device_->isDepthStreamRunning())
00386   {
00387     stopSynchronization();
00388     device_->stopDepthStream();
00389   }
00390   //std::cout << "depth connect cb end..." << std::endl;
00391 }
00392 
00393 void DriverNodelet::irConnectCb()
00394 {
00395   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00396   bool need_ir = pub_ir_.getNumSubscribers() > 0;
00397   
00398   if (need_ir && !device_->isIRStreamRunning())
00399   {
00400     // Can't stream IR and RGB at the same time
00401     if (device_->isImageStreamRunning())
00402     {
00403       NODELET_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
00404     }
00405     else
00406     {
00407       device_->startIRStream();
00408       ir_time_stamp_ = ros::Time::now(); // update stamp for watchdog
00409     }
00410   }
00411   else if (!need_ir)
00412   {
00413     device_->stopIRStream();
00414   }
00415 }
00416 
00417 // If any stream is ready, publish next available image from all streams
00418 // This publishes all available data with a maximum time offset of one frame between any two sources
00419 // Need to have lock to call this, since callbacks can be in different threads
00420 void DriverNodelet::checkFrameCounters()
00421 {
00422     if (max(rgb_frame_counter_, max(depth_frame_counter_, ir_frame_counter_)) > config_.data_skip) {
00423         // Reset all counters after we trigger publish
00424         rgb_frame_counter_   = 0;
00425         depth_frame_counter_ = 0;
00426         ir_frame_counter_    = 0;
00427 
00428         // Trigger publish on all topics
00429         publish_rgb_   = true;
00430         publish_depth_ = true;
00431         publish_ir_    = true;
00432     }
00433 }
00434 
00435 void DriverNodelet::rgbCb(const ImageBuffer& image, void* cookie)
00436 {
00437   ros::Time time = ros::Time::now () + ros::Duration(config_.image_time_offset);
00438   rgb_time_stamp_ = time; // for watchdog
00439 
00440   bool publish = false;
00441   {
00442       boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00443       rgb_frame_counter_++;
00444       checkFrameCounters();
00445       publish = publish_rgb_;
00446 
00447       if (publish)
00448           rgb_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
00449   }
00450 
00451   if (publish)
00452       publishRgbImage(image, time);
00453 
00454   publish_rgb_ = false;
00455 }
00456 
00457 void DriverNodelet::depthCb(const ImageBuffer& depth_image, void* cookie)
00458 {
00459   ros::Time time = ros::Time::now () + ros::Duration(config_.depth_time_offset);
00460   depth_time_stamp_ = time; // for watchdog
00461 
00462   bool publish = false;
00463   {
00464       boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00465       depth_frame_counter_++;
00466       checkFrameCounters();
00467       publish = publish_depth_;
00468 
00469       if (publish)
00470           depth_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
00471   }
00472 
00473   if (publish)
00474       publishDepthImage(depth_image, time);
00475 
00476   publish_depth_ = false;
00477 }
00478 
00479 void DriverNodelet::irCb(const ImageBuffer& ir_image, void* cookie)
00480 {
00481   ros::Time time = ros::Time::now() + ros::Duration(config_.depth_time_offset);
00482   ir_time_stamp_ = time; // for watchdog
00483 
00484   bool publish = false;
00485   {
00486       boost::unique_lock<boost::mutex> counter_lock(counter_mutex_);
00487       ir_frame_counter_++;
00488       checkFrameCounters();
00489       publish = publish_ir_;
00490 
00491       if (publish)
00492           ir_frame_counter_ = 0; // Reset counter if we publish this message to avoid under-throttling
00493   }
00494 
00495   if (publish)
00496       publishIrImage(ir_image, time);
00497   publish_ir_ = false;
00498 }
00499 
00500 void DriverNodelet::publishRgbImage(const ImageBuffer& image, ros::Time time) const
00501 {
00502   //NODELET_INFO_THROTTLE(1.0, "rgb image callback called");
00503   sensor_msgs::ImagePtr rgb_msg = boost::make_shared<sensor_msgs::Image >();
00504   rgb_msg->header.stamp = time;
00505   rgb_msg->header.frame_id = rgb_frame_id_;
00506   rgb_msg->height = image.metadata.height;
00507   rgb_msg->width = image.metadata.width;
00508   switch(image.metadata.video_format) {
00509     case FREENECT_VIDEO_RGB:
00510       rgb_msg->encoding = sensor_msgs::image_encodings::RGB8;
00511       rgb_msg->step = rgb_msg->width * 3;
00512       break;
00513     case FREENECT_VIDEO_BAYER:
00514       rgb_msg->encoding = sensor_msgs::image_encodings::BAYER_GRBG8;
00515       rgb_msg->step = rgb_msg->width;
00516       break;
00517     case FREENECT_VIDEO_YUV_RGB:
00518       rgb_msg->encoding = sensor_msgs::image_encodings::YUV422;
00519       rgb_msg->step = rgb_msg->width * 2;
00520       break;
00521     default:
00522       NODELET_ERROR("Unknown RGB image format received from libfreenect");
00523       // Unknown encoding -- don't publish
00524       return;
00525   }
00526   rgb_msg->data.resize(rgb_msg->height * rgb_msg->step);
00527   fillImage(image, reinterpret_cast<void*>(&rgb_msg->data[0]));
00528   
00529   pub_rgb_.publish(rgb_msg, getRgbCameraInfo(image, time));
00530   if (enable_rgb_diagnostics_)
00531       pub_rgb_freq_->tick();
00532 }
00533 
00534 void DriverNodelet::publishDepthImage(const ImageBuffer& depth, ros::Time time) const
00535 {
00536   //NODELET_INFO_THROTTLE(1.0, "depth image callback called");
00537   bool registered = depth.is_registered;
00538 
00539   sensor_msgs::ImagePtr depth_msg = boost::make_shared<sensor_msgs::Image>();
00540   depth_msg->header.stamp    = time;
00541   depth_msg->encoding        = sensor_msgs::image_encodings::TYPE_16UC1;
00542   depth_msg->height          = depth.metadata.height;
00543   depth_msg->width           = depth.metadata.width;
00544   depth_msg->step            = depth_msg->width * sizeof(short);
00545   depth_msg->data.resize(depth_msg->height * depth_msg->step);
00546 
00547   fillImage(depth, reinterpret_cast<void*>(&depth_msg->data[0]));
00548 
00549   if (z_offset_mm_ != 0)
00550   {
00551     uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg->data[0]);
00552     for (unsigned int i = 0; i < depth_msg->width * depth_msg->height; ++i)
00553       if (data[i] != 0)
00554         data[i] += z_offset_mm_;
00555   }
00556 
00557   if (registered)
00558   {
00559     // Publish RGB camera info and raw depth image to depth_registered/ ns
00560     depth_msg->header.frame_id = rgb_frame_id_;
00561     pub_depth_registered_.publish(depth_msg, getRgbCameraInfo(depth, time));
00562   }
00563   else
00564   {
00565     // Publish depth camera info and raw depth image to depth/ ns
00566     depth_msg->header.frame_id = depth_frame_id_;
00567     pub_depth_.publish(depth_msg, getDepthCameraInfo(depth, time));
00568   }
00569   if (enable_depth_diagnostics_)
00570       pub_depth_freq_->tick();
00571 
00572   // Projector "info" probably only useful for working with disparity images
00573   if (pub_projector_info_.getNumSubscribers() > 0)
00574   {
00575     pub_projector_info_.publish(getProjectorCameraInfo(depth, time));
00576   }
00577 }
00578 
00579 void DriverNodelet::publishIrImage(const ImageBuffer& ir, ros::Time time) const
00580 {
00581   sensor_msgs::ImagePtr ir_msg = boost::make_shared<sensor_msgs::Image>();
00582   ir_msg->header.stamp    = time;
00583   ir_msg->header.frame_id = depth_frame_id_;
00584   ir_msg->encoding        = sensor_msgs::image_encodings::MONO16;
00585   ir_msg->height          = ir.metadata.height;
00586   ir_msg->width           = ir.metadata.width;
00587   ir_msg->step            = ir_msg->width * sizeof(uint16_t);
00588   ir_msg->data.resize(ir_msg->height * ir_msg->step);
00589 
00590   fillImage(ir, reinterpret_cast<void*>(&ir_msg->data[0]));
00591 
00592   pub_ir_.publish(ir_msg, getIrCameraInfo(ir, time));
00593 
00594   if (enable_ir_diagnostics_) 
00595       pub_ir_freq_->tick();
00596 }
00597 
00598 sensor_msgs::CameraInfoPtr DriverNodelet::getDefaultCameraInfo(int width, int height, double f) const
00599 {
00600   sensor_msgs::CameraInfoPtr info = boost::make_shared<sensor_msgs::CameraInfo>();
00601 
00602   info->width  = width;
00603   info->height = height;
00604 
00605   // No distortion
00606   info->D.resize(5, 0.0);
00607   info->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00608 
00609   // Simple camera matrix: square pixels (fx = fy), principal point at center
00610   info->K.assign(0.0);
00611   info->K[0] = info->K[4] = f;
00612   info->K[2] = (width / 2) - 0.5;
00613   // Aspect ratio for the camera center on Kinect (and other devices?) is 4/3
00614   // This formula keeps the principal point the same in VGA and SXGA modes
00615   info->K[5] = (width * (3./8.)) - 0.5;
00616   info->K[8] = 1.0;
00617 
00618   // No separate rectified image plane, so R = I
00619   info->R.assign(0.0);
00620   info->R[0] = info->R[4] = info->R[8] = 1.0;
00621 
00622   // Then P=K(I|0) = (K|0)
00623   info->P.assign(0.0);
00624   info->P[0]  = info->P[5] = f; // fx, fy
00625   info->P[2]  = info->K[2];     // cx
00626   info->P[6]  = info->K[5];     // cy
00627   info->P[10] = 1.0;
00628 
00629   return info;
00630 }
00631 
00633 sensor_msgs::CameraInfoPtr DriverNodelet::getRgbCameraInfo(const ImageBuffer& image, ros::Time time) const
00634 {
00635   sensor_msgs::CameraInfoPtr info;
00636 
00637   if (rgb_info_manager_->isCalibrated())
00638   {
00639     info = boost::make_shared<sensor_msgs::CameraInfo>(rgb_info_manager_->getCameraInfo());
00640   }
00641   else
00642   {
00643     // If uncalibrated, fill in default values
00644     info = getDefaultCameraInfo(image.metadata.width, image.metadata.height, image.focal_length);
00645   }
00646 
00647   // Fill in header
00648   info->header.stamp    = time;
00649   info->header.frame_id = rgb_frame_id_;
00650 
00651   return info;
00652 }
00653 
00654 sensor_msgs::CameraInfoPtr DriverNodelet::getIrCameraInfo(
00655     const ImageBuffer& image, ros::Time time) const {
00656   sensor_msgs::CameraInfoPtr info;
00657 
00658   if (ir_info_manager_->isCalibrated())
00659   {
00660     info = boost::make_shared<sensor_msgs::CameraInfo>(ir_info_manager_->getCameraInfo());
00661   }
00662   else
00663   {
00664     // If uncalibrated, fill in default values
00665     info = getDefaultCameraInfo(image.metadata.width, image.metadata.height, image.focal_length);
00666   }
00667 
00668   // Fill in header
00669   info->header.stamp    = time;
00670   info->header.frame_id = depth_frame_id_;
00671 
00672   return info;
00673 }
00674 
00675 sensor_msgs::CameraInfoPtr DriverNodelet::getDepthCameraInfo(
00676     const ImageBuffer& image, ros::Time time) const {
00677   // The depth image has essentially the same intrinsics as the IR image, BUT the
00678   // principal point is offset by half the size of the hardware correlation window
00679   // (probably 9x9 or 9x7). See http://www.ros.org/wiki/kinect_calibration/technical
00680 
00681   sensor_msgs::CameraInfoPtr info = getIrCameraInfo(image, time);
00682   info->K[2] -= depth_ir_offset_x_; // cx
00683   info->K[5] -= depth_ir_offset_y_; // cy
00684   info->P[2] -= depth_ir_offset_x_; // cx
00685   info->P[6] -= depth_ir_offset_y_; // cy
00686 
00688   return info;
00689 }
00690 
00691 sensor_msgs::CameraInfoPtr DriverNodelet::getProjectorCameraInfo(
00692     const ImageBuffer& image, ros::Time time) const {
00693   // The projector info is simply the depth info with the baseline encoded in the P matrix.
00694   // It's only purpose is to be the "right" camera info to the depth camera's "left" for
00695   // processing disparity images.
00696   sensor_msgs::CameraInfoPtr info = getDepthCameraInfo(image, time);
00697   // Tx = -baseline * fx
00698   info->P[3] = -device_->getBaseline() * info->P[0];
00699   return info;
00700 }
00701 
00702 void DriverNodelet::configCb(Config &config, uint32_t level)
00703 {
00704   depth_ir_offset_x_ = config.depth_ir_offset_x;
00705   depth_ir_offset_y_ = config.depth_ir_offset_y;
00706   z_offset_mm_ = config.z_offset_mm;
00707 
00708   // We need this for the ASUS Xtion Pro
00709   OutputMode old_image_mode, image_mode, compatible_image_mode;
00710   if (device_->hasImageStream ())
00711   {
00712     old_image_mode = device_->getImageOutputMode ();
00713      
00714     // does the device support the new image mode?
00715     image_mode = mapConfigMode2OutputMode (config.image_mode);
00716 
00717     if (!device_->findCompatibleImageMode (image_mode, compatible_image_mode))
00718     {
00719       OutputMode default_mode = device_->getDefaultImageMode();
00720       NODELET_WARN("Could not find any compatible image output mode for %d. "
00721                    "Falling back to default image output mode %d.",
00722                     image_mode,
00723                     default_mode);
00724 
00725       config.image_mode = mapMode2ConfigMode(default_mode);
00726       image_mode = compatible_image_mode = default_mode;
00727     }
00728   }
00729   
00730   OutputMode old_depth_mode, depth_mode, compatible_depth_mode;
00731   old_depth_mode = device_->getDepthOutputMode();
00732   depth_mode = mapConfigMode2OutputMode (config.depth_mode);
00733   if (!device_->findCompatibleDepthMode (depth_mode, compatible_depth_mode))
00734   {
00735     OutputMode default_mode = device_->getDefaultDepthMode();
00736     NODELET_WARN("Could not find any compatible depth output mode for %d. "
00737                  "Falling back to default depth output mode %d.",
00738                   depth_mode,
00739                   default_mode);
00740     
00741     config.depth_mode = mapMode2ConfigMode(default_mode);
00742     depth_mode = compatible_depth_mode = default_mode;
00743   }
00744 
00745   // here everything is fine. Now make the changes
00746   if ( (device_->hasImageStream () && compatible_image_mode != old_image_mode) ||
00747        compatible_depth_mode != old_depth_mode)
00748   {
00749     // streams need to be reset!
00750     stopSynchronization();
00751 
00752     if (device_->hasImageStream () && compatible_image_mode != old_image_mode)
00753       device_->setImageOutputMode (compatible_image_mode);
00754 
00755     if (compatible_depth_mode != old_depth_mode)
00756       device_->setDepthOutputMode (compatible_depth_mode);
00757 
00758     startSynchronization ();
00759   }
00760 
00761   // @todo Run connectCb if registration setting changes
00762   if (device_->isDepthRegistered () && !config.depth_registration)
00763   {
00764     device_->setDepthRegistration (false);
00765   }
00766   else if (!device_->isDepthRegistered () && config.depth_registration)
00767   {
00768     device_->setDepthRegistration (true);
00769   }
00770 
00771   // now we can copy
00772   config_ = config;
00773 }
00774 
00775 void DriverNodelet::startSynchronization()
00776 {
00777   if (device_->isSynchronizationSupported() &&
00778       !device_->isSynchronized() &&
00779       device_->isImageStreamRunning() &&
00780       device_->isDepthStreamRunning() )
00781   {
00782     device_->setSynchronization(true);
00783   }
00784 }
00785 
00786 void DriverNodelet::stopSynchronization()
00787 {
00788   if (device_->isSynchronizationSupported() &&
00789       device_->isSynchronized())
00790   {
00791     device_->setSynchronization(false);
00792   }
00793 }
00794 
00795 void DriverNodelet::updateModeMaps ()
00796 {
00797   OutputMode output_mode;
00798 
00799   output_mode = FREENECT_RESOLUTION_HIGH;
00800   mode2config_map_[output_mode] = Freenect_SXGA;
00801   config2mode_map_[Freenect_SXGA] = output_mode;
00802 
00803   output_mode = FREENECT_RESOLUTION_MEDIUM;
00804   mode2config_map_[output_mode] = Freenect_VGA;
00805   config2mode_map_[Freenect_VGA] = output_mode;
00806 }
00807 
00808 int DriverNodelet::mapMode2ConfigMode (const OutputMode& output_mode) const
00809 {
00810   std::map<OutputMode, int>::const_iterator it = mode2config_map_.find (output_mode);
00811 
00812   if (it == mode2config_map_.end ())
00813   {
00814     NODELET_ERROR ("mode not be found");
00815     exit (-1);
00816   }
00817   else
00818     return it->second;
00819 }
00820 
00821 OutputMode DriverNodelet::mapConfigMode2OutputMode (int mode) const
00822 {
00823   std::map<int, OutputMode>::const_iterator it = config2mode_map_.find (mode);
00824   if (it == config2mode_map_.end ())
00825   {
00826     NODELET_ERROR ("mode %d could not be found", mode);
00827     exit (-1);
00828   }
00829   else
00830     return it->second;
00831 }
00832 
00833 void DriverNodelet::watchDog (const ros::TimerEvent& event)
00834 {
00835   bool timed_out = false;
00836   if (!rgb_time_stamp_.isZero() && device_->isImageStreamRunning()) {
00837     ros::Duration duration = ros::Time::now() - rgb_time_stamp_;
00838     timed_out = timed_out || duration.toSec() > time_out_;
00839   }
00840   if (!depth_time_stamp_.isZero() && device_->isDepthStreamRunning()) {
00841     ros::Duration duration = ros::Time::now() - depth_time_stamp_;
00842     timed_out = timed_out || duration.toSec() > time_out_;
00843   }
00844   if (!ir_time_stamp_.isZero() && device_->isIRStreamRunning()) {
00845     ros::Duration duration = ros::Time::now() - ir_time_stamp_;
00846     timed_out = timed_out || duration.toSec() > time_out_;
00847   }
00848 
00849   if (timed_out) {
00850     ROS_INFO("Device timed out. Flushing device."); 
00851     device_->flushDeviceStreams();
00852   }
00853 
00854 }
00855 
00856 }
00857 
00858 // Register as nodelet
00859 #include <pluginlib/class_list_macros.h>
00860 PLUGINLIB_EXPORT_CLASS(freenect_camera::DriverNodelet, nodelet::Nodelet)


freenect_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu (original openni_camera driver)., Piyush Khandelwal (libfreenect port).
autogenerated on Sat Jun 8 2019 18:26:45