LadybugProcNodelet.cpp

Go to the documentation of this file.

#include "omnicamera/LadybugProcNodelet.h"

#include <boost/format.hpp>
#include <pluginlib/class_list_macros.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <sensor_msgs/image_encodings.h>

#define USE_MT_NODE_HANDLE

namespace omnicamera {

void LadybugProcNodelet::onInit() {
  NODELET_INFO("LadybugImageProcNodelet::onInit");

#ifdef USE_MT_NODE_HANDLE
  ros::NodeHandle nh = getMTNodeHandle();
  ros::NodeHandle pnh = getMTPrivateNodeHandle();
#else
  ros::NodeHandle nh = getNodeHandle();
  ros::NodeHandle pnh = getPrivateNodeHandle();
#endif

  pnh.param("in_width", inWidth, 1616);
  pnh.param("in_height", inHeight, 1232);
  NODELET_INFO("Input image size: %dx%d.", inWidth, inHeight);

  pnh.param("out_width", outWidth, 960);
  pnh.param("out_height", outHeight, 480);
  NODELET_INFO("Output image size: %dx%d.", outWidth, outHeight);

  pnh.param("alpha", alpha, true);
  NODELET_INFO("Alpha blending: %s.", alpha ? "enabled" : "disabled");

  pnh.param("radius", radius, 20);
  NODELET_INFO("Sphere radius: %d.", radius);

  pnh.param("lut_path", lutPath, string("LUT_img_%dx%d_pano_%dx%dr%d_%s%d.pgm"));
  NODELET_INFO("LUT path template: %s.", lutPath.data());
  // Loading look-up tables postponed to first callback to return fast.
  lookupTablesLoaded = false;

  image_transport::ImageTransport it(nh);
  string imageTopic = nh.resolveName("image", true);
  NODELET_INFO("Subscribing to topic %s...", imageTopic.data());
  imageSub = it.subscribe(imageTopic, 1, &LadybugProcNodelet::imageCallback, this);

  outPub = it.advertise("pano", 1);
  camPub = nh.advertise<sensor_msgs::CameraInfo>("camera_info", 1);

  boost::format imageOutFmt("image_%i");
  boost::format camInfoFmt("camera_info_%i");
  for (int i = 0; i < 6; i++) {
    string imageOutTopic = nh.resolveName((imageOutFmt % i).str(), true);
    string camInfoTopic = nh.resolveName((camInfoFmt % i).str(), true);
    NODELET_INFO("Advertising topics %s and %s...", imageOutTopic.data(), camInfoTopic.data());
    outPubs[i] = it.advertise(imageOutTopic, 1);
    camPubs[i] = nh.advertise<sensor_msgs::CameraInfo>(camInfoTopic, 1);
  }
}

void LadybugProcNodelet::imageCallback(const sensor_msgs::ImageConstPtr& msg) {
  NODELET_INFO("LadybugProcNodelet::imageCallback");

  NODELET_INFO("LadybugProcNodelet::imageCallback: Loading look-up tables.");
  if (!lookupTablesLoaded) {
    NODELET_INFO("Loading look-up tables...");
    stitcher.clearLookupTables();
    for (int iCam = 1; iCam <= 6; iCam++) {
      int len = 1024;
      char xPath[len], yPath[len], aPath[len];
      snprintf(xPath, len, lutPath.data(), inWidth, inHeight, outWidth, outHeight, radius, "x", iCam);
      snprintf(yPath, len, lutPath.data(), inWidth, inHeight, outWidth, outHeight, radius, "y", iCam);
      snprintf(aPath, len, lutPath.data(), inWidth, inHeight, outWidth, outHeight, radius, "a", iCam);
      NODELET_INFO("Camera %i look-up tables: %s, %s, %s.", iCam, xPath, yPath, aPath);
      stitcher.addLookupTables(string(xPath), string(yPath), string(aPath));
    }
    lookupTablesLoaded = true;
  }

  NODELET_INFO("LadybugProcNodelet::imageCallback: Converting message to CvImage.");
  cv_bridge::CvImageConstPtr cvPtr;
  try {
    cvPtr = cv_bridge::toCvShare(msg);
  } catch (cv_bridge::Exception& e) {
    NODELET_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

  NODELET_INFO("LadybugProcNodelet::imageCallback: Checking encoding.");
  bool stitch = true;
  if (cvPtr->encoding.compare(sensor_msgs::image_encodings::BGR8)) {
    NODELET_ERROR("Encoding not supported: %s.", cvPtr->encoding.data());
    stitch = false;
  }

  NODELET_INFO("LadybugProcNodelet::imageCallback: Checking image dimensions.");
  if (cvPtr->image.rows != inHeight || cvPtr->image.cols != inWidth) {
    NODELET_ERROR("Unexpected image dimensions: %dx%d.", cvPtr->image.cols, cvPtr->image.rows);
    stitch = false;
  }

  if (stitch && false) {
    NODELET_INFO("Stitching started.");
    stitcher.stitchImage(cvPtr->image, pano);
    NODELET_INFO("Stitching finished.");

    NODELET_INFO("LadybugProcNodelet::imageCallback: Preparing and sending pano message.");
    cv_bridge::CvImage out;
    out.encoding = sensor_msgs::image_encodings::BGR8;
    out.header = msg->header;
    out.image = pano;
    outPub.publish(out.toImageMsg());

    NODELET_INFO("Sending camera info.");
    sensor_msgs::CameraInfo camMsg;
    camMsg.header = out.header;
    camMsg.height = out.image.rows;
    camMsg.width = out.image.cols;
    camPub.publish(camMsg);
  }

  NODELET_INFO("Sawing started.");
  ladySaw.saw(cvPtr->image, cams);
  NODELET_INFO("Sawing finished.");

  vector<int> camsOut = ladySaw.getCamsOut();
  for (int i = 0; i < (int) camsOut.size(); i++) {
    NODELET_INFO("LadybugProcNodelet::imageCallback: Publishing image %i.", i);
    int iCam = camsOut[i];

    cv_bridge::CvImage out;
    out.encoding = cvPtr->encoding;
    out.header = msg->header;
    out.image = cams[i];
    NODELET_INFO("LadybugProcNodelet::imageCallback: before publishing image %i.", i);
    outPubs[iCam].publish(out.toImageMsg());
    NODELET_INFO("LadybugProcNodelet::imageCallback: after publishing image %i.", i);

    sensor_msgs::CameraInfo camMsg;
    camMsg.header = out.header;
    camMsg.height = out.image.rows;
    camMsg.width = out.image.cols;
    camPubs[iCam].publish(camMsg);
  }
}

} // namespace omnicamera

PLUGINLIB_DECLARE_CLASS(omnicamera, ladybug_proc, omnicamera::LadybugProcNodelet, nodelet::Nodelet)