openni_nodelet.cpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011 Willow Garage, Inc.
 *    Suat Gedikli <gedikli@willowgarage.com>
 *    Patrick Michelich <michelich@willowgarage.com>
 *    Radu Bogdan Rusu <rusu@willowgarage.com>
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */
#include "openni_nodelet.h" 
#include "openni_camera/openni_device_kinect.h"
#include "openni_camera/openni_image.h"
#include "openni_camera/openni_depth_image.h"
#include "openni_camera/openni_ir_image.h"
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/distortion_models.h>
#include <boost/algorithm/string/replace.hpp>
#include "names.h"

using namespace std;
using namespace openni_wrapper;
namespace openni_camera
{
inline bool operator == (const XnMapOutputMode& mode1, const XnMapOutputMode& mode2)
{
  return (mode1.nXRes == mode2.nXRes && mode1.nYRes == mode2.nYRes && mode1.nFPS == mode2.nFPS);
}
inline bool operator != (const XnMapOutputMode& mode1, const XnMapOutputMode& mode2)
{
  return !(mode1 == mode2);
}

OpenNINodelet::~OpenNINodelet ()
{
  device_->stopDepthStream ();
  device_->stopImageStream ();
}

void OpenNINodelet::onInit ()
{
  ros::NodeHandle& nh       = getNodeHandle();        // topics
  ros::NodeHandle& param_nh = getPrivateNodeHandle(); // parameters
  image_transport::ImageTransport it(nh);

  updateModeMaps(); // Register mappings from config modes to XnModes and vice versa
  setupDevice();    // Initialize the device(s), but don't start the streams yet

  // Initialize dynamic reconfigure
  reconfigure_server_.reset( new ReconfigureServer(param_nh) );
  reconfigure_server_->setCallback(boost::bind(&OpenNINodelet::configCb, this, _1, _2));

  // Camera TF frames
  param_nh.param("rgb_frame_id",   rgb_frame_id_,   string("/openni_rgb_optical_frame"));
  param_nh.param("depth_frame_id", depth_frame_id_, string("/openni_depth_optical_frame"));
  NODELET_INFO("rgb_frame_id = '%s' ",   rgb_frame_id_.c_str());
  NODELET_INFO("depth_frame_id = '%s' ", depth_frame_id_.c_str());

  // Pixel offset between depth and IR images.
  // By default assume offset of (5,4) from 9x7 correlation window.
  param_nh.param("depth_ir_offset_x", depth_ir_offset_x_, 5.0);
  param_nh.param("depth_ir_offset_y", depth_ir_offset_y_, 4.0);

  // If the user has calibrated, load camera parameters from files
  rgb_info_manager_ = boost::make_shared<CameraInfoManager>(ros::NodeHandle(nh, "rgb"));
  ir_info_manager_  = boost::make_shared<CameraInfoManager>(ros::NodeHandle(nh, "ir"));
  std::string serial_number = device_->getSerialNumber();
  std::string rgb_name, ir_name;
  if (serial_number.empty())
  {
    rgb_name = "rgb";
    ir_name  = "depth"; 
  }
  else
  {
    rgb_name = "rgb_"   + serial_number;
    ir_name  = "depth_" + serial_number;
  }
  rgb_info_manager_->setCameraName(rgb_name);
  ir_info_manager_ ->setCameraName(ir_name);

  // Replace %s in URL string with serial number, to make working with multiple
  // calibrated devices easier
  std::string info_url;
  if (param_nh.getParam("rgb_camera_info_url", info_url))
  {
    boost::replace_all(info_url, "%s", rgb_name);
    if (!rgb_info_manager_->loadCameraInfo(info_url))
      NODELET_WARN("Failed to load RGB camera calibration, using default parameters");
  }
  else
    NODELET_INFO("RGB camera uncalibrated, using default parameters");
  
  if (param_nh.getParam("depth_camera_info_url", info_url))
  {
    boost::replace_all(info_url, "%s", ir_name);
    if (!ir_info_manager_->loadCameraInfo(info_url))
      NODELET_WARN("Failed to load IR camera calibration, using default parameters");
  }
  else
    NODELET_INFO("IR camera uncalibrated, using default parameters");

  // Advertise all published topics
  image_transport::SubscriberStatusCallback itssc = boost::bind(&OpenNINodelet::connectCb, this);
  ros::SubscriberStatusCallback rssc = boost::bind(&OpenNINodelet::connectCb, this);
  typedef sensor_msgs::CameraInfo CI;
  pub_rgb_raw_               = it.advertise    (resolve(nh, "rgb/image_raw"),                1, itssc, itssc);
  pub_rgb_mono_              = it.advertise    (resolve(nh, "rgb/image_mono"),               1, itssc, itssc);
  pub_rgb_color_             = it.advertise    (resolve(nh, "rgb/image_color"),              1, itssc, itssc);
  pub_rgb_info_              = nh.advertise<CI>(resolve(nh, "rgb/camera_info"),              1, rssc,  rssc);  
  pub_depth_                 = it.advertise    (resolve(nh, "depth/image_raw"),              1, itssc, itssc);
  pub_depth_info_            = nh.advertise<CI>(resolve(nh, "depth/camera_info"),            1, rssc,  rssc);
  pub_ir_                    = it.advertise    (resolve(nh, "ir/image_raw"),                 1, itssc, itssc);
  pub_ir_info_               = nh.advertise<CI>(resolve(nh, "ir/camera_info"),               1, rssc,  rssc);
  pub_depth_registered_      = it.advertise    (resolve(nh, "depth_registered/image_raw"),   1, itssc, itssc);
  pub_depth_registered_info_ = nh.advertise<CI>(resolve(nh, "depth_registered/camera_info"), 1, rssc,  rssc);

  // Create watch dog timer callback
  if (param_nh.getParam("time_out", time_out_) && time_out_ > 0.0)
  {
    time_stamp_ = ros::Time(0,0);
    watch_dog_timer_ = nh.createTimer(ros::Duration(time_out_), &OpenNINodelet::watchDog, this);
  }
}

void OpenNINodelet::setupDevice ()
{
  // Initialize the openni device
  OpenNIDriver& driver = OpenNIDriver::getInstance ();

  do {
    driver.updateDeviceList ();

    if (driver.getNumberDevices () == 0)
    {
      NODELET_INFO ("No devices connected.... waiting for devices to be connected");
      sleep(3);
      continue;
    }

    NODELET_INFO ("Number devices connected: %d", driver.getNumberDevices ());
    for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
    {
      NODELET_INFO ("%u. device on bus %03u:%02u is a %s (%03x) from %s (%03x) with serial id \'%s\'"
                , deviceIdx + 1, driver.getBus (deviceIdx), driver.getAddress (deviceIdx)
                , driver.getProductName (deviceIdx), driver.getProductID (deviceIdx), driver.getVendorName (deviceIdx)
                , driver.getVendorID (deviceIdx), driver.getSerialNumber (deviceIdx));
    }

    try {
      string device_id;
      if (!getPrivateNodeHandle().getParam("device_id", device_id))
      {
        NODELET_WARN ("~device_id is not set! Using first device.");
        device_ = driver.getDeviceByIndex (0);
      }
      else if (device_id.find ('@') != string::npos)
      {
        size_t pos = device_id.find ('@');
        unsigned bus = atoi (device_id.substr (0, pos).c_str ());
        unsigned address = atoi (device_id.substr (pos + 1, device_id.length () - pos - 1).c_str ());
        NODELET_INFO ("Searching for device with bus@address = %d@%d", bus, address);
        device_ = driver.getDeviceByAddress (bus, address);
      }
      else if (device_id[0] == '#')
      {
        unsigned index = atoi (device_id.c_str () + 1);
        NODELET_INFO ("Searching for device with index = %d", index);
        device_ = driver.getDeviceByIndex (index - 1);
      }
      else
      {
        NODELET_INFO ("Searching for device with serial number = '%s'", device_id.c_str ());
        device_ = driver.getDeviceBySerialNumber (device_id);
      }
    }
    catch (const OpenNIException& exception)
    {
      if (!device_)
      {
        NODELET_INFO ("No matching device found.... waiting for devices. Reason: %s", exception.what ());
        sleep (3);
        continue;
      }
      else
      {
        NODELET_ERROR ("Could not retrieve device. Reason: %s", exception.what ());
        exit (-1);
      }
    }
  } while (!device_);

  NODELET_INFO ("Opened '%s' on bus %d:%d with serial number '%s'", device_->getProductName (),
                device_->getBus (), device_->getAddress (), device_->getSerialNumber ());

  device_->registerImageCallback(&OpenNINodelet::imageCb, *this);
  device_->registerDepthCallback(&OpenNINodelet::depthCb, *this);
  device_->registerIRCallback   (&OpenNINodelet::irCb,    *this);
}

void OpenNINodelet::imageCb(boost::shared_ptr<openni_wrapper::Image> image, void* cookie)
{
  ros::Time time = ros::Time::now () + ros::Duration(config_.image_time_offset);
  time_stamp_ = time; // for watchdog

  if (pub_rgb_info_.getNumSubscribers() > 0)
    pub_rgb_info_.publish(getRgbCameraInfo(time));

  if (pub_rgb_raw_.getNumSubscribers () > 0)
    publishRgbImageRaw (*image, time);

  if (pub_rgb_mono_.getNumSubscribers () > 0)
    publishGrayImage (*image, time);

  if (pub_rgb_color_.getNumSubscribers () > 0)
    publishRgbImage (*image, time);
}

void OpenNINodelet::depthCb(boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void* cookie)
{
  ros::Time time = ros::Time::now () + ros::Duration(config_.depth_time_offset);
  time_stamp_ = time; // for watchdog

  if (device_->isDepthRegistered())
  {
    // Publish RGB camera info and raw depth image to camera/depth_registered/
    if (pub_depth_registered_info_.getNumSubscribers() > 0)
      pub_depth_registered_info_.publish(getRgbCameraInfo(time));

    if (pub_depth_registered_.getNumSubscribers() > 0)
      publishDepthImageRaw(*depth_image, time, rgb_frame_id_, pub_depth_registered_);
  }
  else
  {
    // Publish depth camera info and raw depth image to camera/depth/
    if (pub_depth_info_.getNumSubscribers() > 0)
      pub_depth_info_.publish(getDepthCameraInfo(time));

    if (pub_depth_.getNumSubscribers() > 0)
      publishDepthImageRaw(*depth_image, time, depth_frame_id_, pub_depth_);
  }
}

void OpenNINodelet::irCb(boost::shared_ptr<openni_wrapper::IRImage> ir_image, void* cookie)
{
  ros::Time time = ros::Time::now() + ros::Duration(config_.depth_time_offset);
  time_stamp_ = time; // for watchdog

  if (pub_ir_info_.getNumSubscribers() > 0)
    pub_ir_info_.publish(getIrCameraInfo(time));
  
  if (pub_ir_.getNumSubscribers() > 0)
    publishIrImage(*ir_image, time);
}

void OpenNINodelet::connectCb()
{
  // Check if we need to start/stop any stream
  if (isImageStreamRequired () && !device_->isImageStreamRunning ())
  {
    // Can't stream IR and RGB at the same time. Give RGB preference.
    if (device_->isIRStreamRunning())
      device_->stopIRStream();
    
    device_->startImageStream ();
    startSynchronization ();
    time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
  }
  else if (!isImageStreamRequired () && device_->isImageStreamRunning ())
  {
    stopSynchronization ();
    device_->stopImageStream ();
  }

  if (isDepthStreamRequired () && !device_->isDepthStreamRunning ())
  {
    //NODELET_INFO("Starting depth stream");
    device_->startDepthStream ();
    startSynchronization ();
    time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
  }
  else if ( !isDepthStreamRequired () && device_->isDepthStreamRunning ())
  {
    //NODELET_INFO("Stopping depth stream");
    stopSynchronization ();
    device_->stopDepthStream ();
  }

  if (isIrStreamRequired() && !device_->isIRStreamRunning())
  {
    // Can't stream IR and RGB at the same time
    if (device_->isImageStreamRunning())
    {
      NODELET_ERROR("Cannot stream RGB and IR at the same time. Streaming RGB only.");
    }
    else
    {
      device_->startIRStream();
      time_stamp_ = ros::Time(0,0); // starting an additional stream blocks for a while, could upset watchdog
    }
  }
  else if (!isIrStreamRequired() && device_->isIRStreamRunning())
  {
    device_->stopIRStream();
  }
}

void OpenNINodelet::publishRgbImageRaw (const Image& image, ros::Time time) const
{
  sensor_msgs::ImagePtr rgb_msg = boost::make_shared<sensor_msgs::Image > ();
  rgb_msg->header.stamp = time;
  rgb_msg->header.frame_id = rgb_frame_id_;
  if (image.getEncoding () == openni_wrapper::Image::BAYER_GRBG)
  {
    rgb_msg->encoding = sensor_msgs::image_encodings::BAYER_GRBG8;
    rgb_msg->step = image_width_;
  }
  else if (image.getEncoding () == openni_wrapper::Image::YUV422)
  {
    rgb_msg->encoding = "yuv422"; 
    rgb_msg->step = image_width_ * 2; // 4 bytes for 2 pixels
  }
  rgb_msg->height = image_height_;
  rgb_msg->width = image_width_;
  rgb_msg->data.resize (rgb_msg->height * rgb_msg->step);
  image.fillRaw (&rgb_msg->data[0]);
  
  pub_rgb_raw_.publish (rgb_msg);
}

void OpenNINodelet::publishGrayImage (const Image& image, ros::Time time) const
{
  sensor_msgs::ImagePtr gray_msg = boost::make_shared<sensor_msgs::Image > ();
  gray_msg->header.stamp = time;
  gray_msg->header.frame_id = rgb_frame_id_;
  gray_msg->encoding = sensor_msgs::image_encodings::MONO8;
  gray_msg->height = image_height_;
  gray_msg->width = image_width_;
  gray_msg->step = image_width_;
  gray_msg->data.resize (gray_msg->height * gray_msg->step);
  image.fillGrayscale (gray_msg->width, gray_msg->height, &gray_msg->data[0], gray_msg->step);

  pub_rgb_mono_.publish (gray_msg);
}

void OpenNINodelet::publishRgbImage (const Image& image, ros::Time time) const
{
  sensor_msgs::ImagePtr rgb_msg = boost::make_shared<sensor_msgs::Image > ();
  rgb_msg->header.stamp = time;
  rgb_msg->header.frame_id = rgb_frame_id_;
  rgb_msg->encoding = sensor_msgs::image_encodings::RGB8;
  rgb_msg->height = image_height_;
  rgb_msg->width = image_width_;
  rgb_msg->step = image_width_ * 3;
  rgb_msg->data.resize (rgb_msg->height * rgb_msg->step);
  image.fillRGB (rgb_msg->width, rgb_msg->height, &rgb_msg->data[0], rgb_msg->step);

  pub_rgb_color_.publish (rgb_msg);
}

void OpenNINodelet::publishDepthImageRaw (const openni_wrapper::DepthImage& depth,
                                          ros::Time time, const std::string& frame_id,
                                          const image_transport::Publisher& pub) const
{
  sensor_msgs::ImagePtr depth_msg = boost::make_shared<sensor_msgs::Image> ();
  depth_msg->header.stamp    = time;
  depth_msg->header.frame_id = frame_id;
  depth_msg->encoding        = sensor_msgs::image_encodings::TYPE_16UC1;
  depth_msg->height          = depth_height_;
  depth_msg->width           = depth_width_;
  depth_msg->step            = depth_msg->width * sizeof (short);
  depth_msg->data.resize (depth_msg->height * depth_msg->step);

  depth.fillDepthImageRaw (depth_width_, depth_height_, reinterpret_cast<short*>(&depth_msg->data[0]),
                           depth_msg->step);

  pub.publish (depth_msg);
}

void OpenNINodelet::publishIrImage(const openni_wrapper::IRImage& ir, ros::Time time) const
{
  sensor_msgs::ImagePtr ir_msg = boost::make_shared<sensor_msgs::Image>();
  ir_msg->header.stamp    = time;
  ir_msg->header.frame_id = depth_frame_id_;
  ir_msg->encoding        = sensor_msgs::image_encodings::MONO16;
  ir_msg->height          = ir.getHeight();
  ir_msg->width           = ir.getWidth();
  ir_msg->step            = ir_msg->width * sizeof(uint16_t);
  ir_msg->data.resize(ir_msg->height * ir_msg->step);

  ir.fillRaw(reinterpret_cast<unsigned short*>(&ir_msg->data[0]));

  pub_ir_.publish(ir_msg);
}

sensor_msgs::CameraInfoPtr OpenNINodelet::getDefaultCameraInfo(int width, int height, double f) const
{
  sensor_msgs::CameraInfoPtr info = boost::make_shared<sensor_msgs::CameraInfo>();

  info->width  = width;
  info->height = height;

  // No distortion
  info->D.resize(5, 0.0);
  info->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;

  // Simple camera matrix: square pixels (fx = fy), principal point at center
  info->K.assign(0.0);
  info->K[0] = info->K[4] = f;
  info->K[2] = (width / 2) - 0.5;
  // Aspect ratio for the camera center on Kinect (and other devices?) is 4/3
  // This formula keeps the principal point the same in VGA and SXGA modes
  info->K[5] = (width * (3./8.)) - 0.5;
  info->K[8] = 1.0;

  // No separate rectified image plane, so R = I
  info->R.assign(0.0);
  info->R[0] = info->R[4] = info->R[8] = 1.0;

  // Then P=K(I|0) = (K|0)
  info->P.assign(0.0);
  info->P[0]  = info->P[5] = f; // fx, fy
  info->P[2]  = info->K[2];     // cx
  info->P[6]  = info->K[5];     // cy
  info->P[10] = 1.0;

  return info;
}

sensor_msgs::CameraInfoPtr OpenNINodelet::getRgbCameraInfo(ros::Time time) const
{
  sensor_msgs::CameraInfoPtr info;

  if (rgb_info_manager_->isCalibrated())
  {
    info = boost::make_shared<sensor_msgs::CameraInfo>(rgb_info_manager_->getCameraInfo());
  }
  else
  {
    // If uncalibrated, fill in default values
    info = getDefaultCameraInfo(image_width_, image_height_, device_->getImageFocalLength(image_width_));
  }

  // Fill in header
  info->header.stamp    = time;
  info->header.frame_id = rgb_frame_id_;

  return info;
}

sensor_msgs::CameraInfoPtr OpenNINodelet::getIrCameraInfo(ros::Time time) const
{
  sensor_msgs::CameraInfoPtr info;

  if (ir_info_manager_->isCalibrated())
  {
    info = boost::make_shared<sensor_msgs::CameraInfo>(ir_info_manager_->getCameraInfo());
  }
  else
  {
    // If uncalibrated, fill in default values
    info = getDefaultCameraInfo(depth_width_, depth_height_, device_->getDepthFocalLength(depth_width_));
  }

  // Fill in header
  info->header.stamp    = time;
  info->header.frame_id = depth_frame_id_;

  return info;
}

sensor_msgs::CameraInfoPtr OpenNINodelet::getDepthCameraInfo(ros::Time time) const
{
  // The depth image has essentially the same intrinsics as the IR image, BUT the
  // principal point is offset by half the size of the hardware correlation window
  // (probably 9x9 or 9x7). See http://www.ros.org/wiki/kinect_calibration/technical

  sensor_msgs::CameraInfoPtr info = getIrCameraInfo(time);
  info->K[2] -= depth_ir_offset_x_; // cx
  info->K[5] -= depth_ir_offset_y_; // cy
  info->P[2] -= depth_ir_offset_x_; // cx
  info->P[6] -= depth_ir_offset_y_; // cy

  return info;
}

void OpenNINodelet::configCb(Config &config, uint32_t level)
{
  XnMapOutputMode old_image_mode = device_->getImageOutputMode ();
  XnMapOutputMode old_depth_mode = device_->getDepthOutputMode ();

  // does the device support the new image mode?
  XnMapOutputMode image_mode, compatible_image_mode;
  image_mode = mapConfigMode2XnMode (config.image_mode);

  if (!device_->findCompatibleImageMode (image_mode, compatible_image_mode))
  {
    XnMapOutputMode default_mode = device_->getDefaultImageMode();
    NODELET_WARN("Could not find any compatible image output mode for %d x %d @ %d. "
                 "Falling back to default image output mode %d x %d @ %d.",
                  image_mode.nXRes, image_mode.nYRes, image_mode.nFPS,
                  default_mode.nXRes, default_mode.nYRes, default_mode.nFPS);

    config.image_mode = mapXnMode2ConfigMode(default_mode);
    image_mode = compatible_image_mode = default_mode;
  }

  XnMapOutputMode depth_mode, compatible_depth_mode;
  depth_mode = mapConfigMode2XnMode (config.depth_mode);
  if (!device_->findCompatibleDepthMode (depth_mode, compatible_depth_mode))
  {
    XnMapOutputMode default_mode = device_->getDefaultDepthMode();
    NODELET_WARN ("Could not find any compatible depth output mode for %d x %d @ %d. "
                  "Falling back to default depth output mode %d x %d @ %d.",
                  depth_mode.nXRes, depth_mode.nYRes, depth_mode.nFPS,
                  default_mode.nXRes, default_mode.nYRes, default_mode.nFPS);
    
    config.depth_mode = mapXnMode2ConfigMode(default_mode);
    depth_mode = compatible_depth_mode = default_mode;
  }

  DeviceKinect* kinect = dynamic_cast<DeviceKinect*> (device_.get ());
  if (kinect)
  {
    switch (config.debayering)
    {
      case OpenNI_Bilinear:
        kinect->setDebayeringMethod (ImageBayerGRBG::Bilinear);
        break;
      case OpenNI_EdgeAware:
        kinect->setDebayeringMethod (ImageBayerGRBG::EdgeAware);
        break;
      case OpenNI_EdgeAwareWeighted:
        kinect->setDebayeringMethod (ImageBayerGRBG::EdgeAwareWeighted);
        break;
    }
  }
  else if (config.debayering != config_.debayering) // this was selected explicitly
  {
    NODELET_WARN ("%s does not output bayer images. Selection has no affect.", device_->getProductName () );
  }

  // here everything is fine. Now make the changes
  if (compatible_image_mode != old_image_mode || compatible_depth_mode != old_depth_mode)
  {
    // streams need to be reset!
    stopSynchronization();

    if (compatible_image_mode != old_image_mode)
      device_->setImageOutputMode (compatible_image_mode);

    if (compatible_depth_mode != old_depth_mode)
      device_->setDepthOutputMode (compatible_depth_mode);

    startSynchronization ();
  }

  // Desired dimensions may require decimation from the hardware-compatible ones
  image_width_  = image_mode.nXRes;
  image_height_ = image_mode.nYRes;
  depth_width_  = depth_mode.nXRes;
  depth_height_ = depth_mode.nYRes;

  if (device_->isDepthRegistered () && !config.depth_registration)
  {
    device_->setDepthRegistration (false);
  }
  else if (!device_->isDepthRegistered () && config.depth_registration)
  {
    device_->setDepthRegistration (true);
  }

  // now we can copy
  config_ = config;
}

void OpenNINodelet::startSynchronization ()
{
  if (device_->isSynchronizationSupported () && !device_->isSynchronized () &&
      device_->getImageOutputMode ().nFPS == device_->getDepthOutputMode ().nFPS &&
      device_->isImageStreamRunning () && device_->isDepthStreamRunning () )
    device_->setSynchronization (true);
}

void OpenNINodelet::stopSynchronization ()
{
  if (device_->isSynchronizationSupported () && device_->isSynchronized ())
    device_->setSynchronization (false);
}

void OpenNINodelet::updateModeMaps ()
{
  XnMapOutputMode output_mode;

  output_mode.nXRes = XN_SXGA_X_RES;
  output_mode.nYRes = XN_SXGA_Y_RES;
  output_mode.nFPS  = 15;
  xn2config_map_[output_mode] = OpenNI_SXGA_15Hz;
  config2xn_map_[OpenNI_SXGA_15Hz] = output_mode;

  output_mode.nXRes = XN_VGA_X_RES;
  output_mode.nYRes = XN_VGA_Y_RES;
  output_mode.nFPS  = 25;
  xn2config_map_[output_mode] = OpenNI_VGA_25Hz;
  config2xn_map_[OpenNI_VGA_25Hz] = output_mode;
  output_mode.nFPS  = 30;
  xn2config_map_[output_mode] = OpenNI_VGA_30Hz;
  config2xn_map_[OpenNI_VGA_30Hz] = output_mode;

  output_mode.nXRes = XN_QVGA_X_RES;
  output_mode.nYRes = XN_QVGA_Y_RES;
  output_mode.nFPS  = 25;
  xn2config_map_[output_mode] = OpenNI_QVGA_25Hz;
  config2xn_map_[OpenNI_QVGA_25Hz] = output_mode;
  output_mode.nFPS  = 30;
  xn2config_map_[output_mode] = OpenNI_QVGA_30Hz;
  config2xn_map_[OpenNI_QVGA_30Hz] = output_mode;
  output_mode.nFPS  = 60;
  xn2config_map_[output_mode] = OpenNI_QVGA_60Hz;
  config2xn_map_[OpenNI_QVGA_60Hz] = output_mode;

  output_mode.nXRes = XN_QQVGA_X_RES;
  output_mode.nYRes = XN_QQVGA_Y_RES;
  output_mode.nFPS  = 25;
  xn2config_map_[output_mode] = OpenNI_QQVGA_25Hz;
  config2xn_map_[OpenNI_QQVGA_25Hz] = output_mode;
  output_mode.nFPS  = 30;
  xn2config_map_[output_mode] = OpenNI_QQVGA_30Hz;
  config2xn_map_[OpenNI_QQVGA_30Hz] = output_mode;
  output_mode.nFPS  = 60;
  xn2config_map_[output_mode] = OpenNI_QQVGA_60Hz;
  config2xn_map_[OpenNI_QQVGA_60Hz] = output_mode;
}

int OpenNINodelet::mapXnMode2ConfigMode (const XnMapOutputMode& output_mode) const
{
  std::map<XnMapOutputMode, int, modeComp>::const_iterator it = xn2config_map_.find (output_mode);

  if (it == xn2config_map_.end ())
  {
    NODELET_ERROR ("mode %dx%d@%d could not be found", output_mode.nXRes, output_mode.nYRes, output_mode.nFPS);
    exit (-1);
  }
  else
    return it->second;
}

XnMapOutputMode OpenNINodelet::mapConfigMode2XnMode (int mode) const
{
  std::map<int, XnMapOutputMode>::const_iterator it = config2xn_map_.find (mode);
  if (it == config2xn_map_.end ())
  {
    NODELET_ERROR ("mode %d could not be found", mode);
    exit (-1);
  }
  else
    return it->second;
}

void OpenNINodelet::watchDog (const ros::TimerEvent& event)
{
  if ( !time_stamp_.isZero() && (device_->isDepthStreamRunning() || device_->isImageStreamRunning()) )
  {
    ros::Duration duration = ros::Time::now() - time_stamp_;
    if (duration.toSec() >= time_out_)
    {
      NODELET_ERROR("Timeout");
      watch_dog_timer_.stop();
      throw std::runtime_error("Timeout occured in OpenNINodelet");
    }
  }
}

}

// Register as nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_DECLARE_CLASS (openni_camera, driver, openni_camera::OpenNINodelet, nodelet::Nodelet);

---

openni_camera_unstable
Author(s): Suat Gedikli, Patrick Mihelich, Radu Bogdan Rusu
autogenerated on Fri Jan 11 09:14:58 2013