crop_decimate.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include "names.h"


namespace openni_camera {

class CropDecimateNodelet : public nodelet::Nodelet
{
  boost::shared_ptr<image_transport::ImageTransport> it_;
  image_transport::CameraSubscriber sub_;
  image_transport::CameraPublisher pub_;
  
  int queue_size_;
  int decimation_x;
  int decimation_y;

  virtual void onInit();

  void connectCb();

  void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
               const sensor_msgs::CameraInfoConstPtr& info_msg);
};

void CropDecimateNodelet::onInit()
{
  ros::NodeHandle& nh       = getNodeHandle();
  ros::NodeHandle& param_nh = getPrivateNodeHandle();
  it_.reset(new image_transport::ImageTransport(nh));

  // Read parameters
  param_nh.param("queue_size", queue_size_, 5);
  param_nh.param("decimation_x", decimation_x, 1);
  param_nh.param("decimation_y", decimation_y, 1);

  // Monitor whether anyone is subscribed to the output
  image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropDecimateNodelet::connectCb, this);
  pub_ = it_->advertiseCamera(resolve(nh, "camera_out/image_raw"),  1, connect_cb, connect_cb);
}

// Handles (un)subscribing when clients (un)subscribe
void CropDecimateNodelet::connectCb()
{
  if (pub_.getNumSubscribers() == 0)
    sub_.shutdown();
  else if (!sub_)
    sub_ = it_->subscribeCamera(resolve(getNodeHandle(), "camera/image_raw"),
                                queue_size_, &CropDecimateNodelet::imageCb, this);
}

void CropDecimateNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
                                  const sensor_msgs::CameraInfoConstPtr& info_msg)
{
  if (decimation_x == 1 && decimation_y == 1)
  {
    pub_.publish(image_msg, info_msg);
    return;
  }

  if (decimation_x % 2 != 0 || decimation_y % 2 != 0)
  {
    NODELET_ERROR_THROTTLE(2, "Odd binning not supported yet");
    return;
  }

  if (!sensor_msgs::image_encodings::isBayer(image_msg->encoding))
  {
    NODELET_ERROR_THROTTLE(2, "Only Bayer encodings supported currenty");
    return;
  }


  // Create updated CameraInfo message
  sensor_msgs::CameraInfoPtr out_info = boost::make_shared<sensor_msgs::CameraInfo>(*info_msg);
  out_info->binning_x = std::max((int)out_info->binning_x, 1) * decimation_x;
  out_info->binning_y = std::max((int)out_info->binning_y, 1) * decimation_y;

  // Allocate new image message
  sensor_msgs::ImagePtr out_image = boost::make_shared<sensor_msgs::Image>();
  out_image->header   = image_msg->header;
  out_image->height   = image_msg->height / decimation_y;
  out_image->width    = image_msg->width  / decimation_x;
  out_image->encoding = sensor_msgs::image_encodings::BGR8; // for bayer
  out_image->step     = out_image->width * 3;
  out_image->data.resize(out_image->height * out_image->step);

  // Compute offsets to color elements in a 2x2 bayer pixel group
  int R, G1, G2, B;
  if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB8)
  {
    R  = 0;
    G1 = 1;
    G2 = image_msg->step;
    B  = image_msg->step + 1;
  }
  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR8)
  {
    B  = 0;
    G1 = 1;
    G2 = image_msg->step;
    R  = image_msg->step + 1;
  }
  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG8)
  {
    G1 = 0;
    B  = 1;
    R  = image_msg->step;
    G2 = image_msg->step + 1;
  }
  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG8)
  {
    G1 = 0;
    R  = 1;
    B  = image_msg->step;
    G2 = image_msg->step + 1;
  }
  else
  {
    NODELET_ERROR_THROTTLE(2, "Unrecognized Bayer encoding '%s'", image_msg->encoding.c_str());
    return;
  }

  // Hit only the pixel groups we need
  int bayer_step = decimation_y * image_msg->step;
  int bayer_skip = decimation_x;
  const uint8_t* bayer_row = &image_msg->data[0];
  uint8_t* bgr_buffer = &out_image->data[0];

  // Downsample and debayer at once
  for (int y = 0; y < (int)out_image->height; ++y, bayer_row += bayer_step)
  {
    const uint8_t* bayer_buffer = bayer_row;
    for (int x = 0; x < (int)out_image->width; ++x, bayer_buffer += bayer_skip, bgr_buffer += 3)
    {
      bgr_buffer[0] = bayer_buffer[B];
      bgr_buffer[1] = (bayer_buffer[G1] + bayer_buffer[G2]) / 2;
      bgr_buffer[2] = bayer_buffer[R];
    }
  }

  pub_.publish(out_image, out_info);
}

} // namespace openni_camera

// Register nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_DECLARE_CLASS(openni_camera, crop_decimate, openni_camera::CropDecimateNodelet, nodelet::Nodelet)

---

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