raw_image_publisher.cpp

Go to the documentation of this file.

/*
 * raw_image_publisher_node.cpp
 *
 *  Created on: Aug 27, 2010
 *      Author: petrito1@cmp.felk.cvut.cz
 */
#include <image_transport/image_transport.h>
#include <math.h>
#include <ros/ros.h>
#include <ros/time.h>
// #include <roslib/Header.h>
// #include <roslib/Clock.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sys/param.h>
#include <sys/times.h>

#include "Camera.h"
#include "CameraContext.h"
#include "CameraException.h"

using std::string;

using nifti::dc1394::Camera;
using nifti::dc1394::CameraContext;
using nifti::dc1394::CameraException;

namespace {
//const std::string rawTopic = "/unwrapper/unwrapped";
const std::string rawTopic = "raw";
}

int main(int argc, char** argv) {

  ros::init(argc, argv, "raw_image_publisher");
  ros::NodeHandle nodeHandle;

  image_transport::ImageTransport it(nodeHandle);
  image_transport::Publisher pub = it.advertise(rawTopic, 4);

  // Will be cleared at the end of the try block if no exception occurs.
  bool exceptionOccurred = true;
  try {

    CameraContext context;

    Camera camera = context.getCamera();
    camera.resetBus();
    //    camera.reset();
    camera.setup();
    camera.setupCapture();
    camera.startVideoTransmission();

    struct tms t1;
    long r1 = 0l;
    uint32_t seq = 0;
    while (nodeHandle.ok()) {

      r1 = times(&t1);
      ROS_DEBUG("Dequeuing frame...\n");
      dc1394video_frame_t* dequeuedFrame = NULL;
      dequeuedFrame = camera.dequeueFrame(DC1394_CAPTURE_POLICY_WAIT);

      if (!dequeuedFrame) {
        continue;
      } else if (camera.isFrameCorrupt(dequeuedFrame)) {
        ROS_WARN("Frame corrupt.\n");
        camera.enqueueFrameLater(dequeuedFrame);
        camera.enqueueFrames();
        continue;
      }

      // Init the sensor message and process the frame.
      //      sensor_msgs::CompressedImage compressedImageMsg;
      sensor_msgs::Image imageMsg;

      //imageMsg.header.frame_id = "0";
      imageMsg.header.frame_id = "omni_frame";

      imageMsg.header.seq = seq++;
      imageMsg.header.stamp = ros::Time((double) dequeuedFrame->timestamp * 1000000);

      //      imageMsg.data.assign(dequeuedFrame->image, dequeuedFrame->image + dequeuedFrame->image_bytes);
      imageMsg.data.assign(dequeuedFrame->image, dequeuedFrame->image + dequeuedFrame->image_bytes);

      //       BAYER_RGGB8 does not work in CvBridge on subscriber side.
//      imageMsg.encoding = sensor_msgs::image_encodings::BAYER_RGGB8;
            imageMsg.encoding = sensor_msgs::image_encodings::MONO8;
      imageMsg.width = dequeuedFrame->size[0];
      imageMsg.height = dequeuedFrame->size[1];
      imageMsg.is_bigendian = false;
      // Try auto step because of CvBridge.
            imageMsg.step = dequeuedFrame->stride;
//      imageMsg.step = 0;
//                  imageMsg.step = 1616;

      pub.publish(imageMsg);

      camera.enqueueFrameLater(dequeuedFrame);
      camera.enqueueFrames();
    }

    // Clean up.
    camera.stopVideoTransmission();
    camera.stopCapture();
    camera.releaseAll();

    exceptionOccurred = false;

  } catch (CameraException& e) {
    ROS_ERROR("Camera exception: %s %s", e.what(), e.getErrorString().data());
  } catch (std::runtime_error& e) {
    ROS_ERROR("Runtime error: %s", e.what());
  } catch (...) {
    ROS_ERROR("Unexpected exception occurred.");
  }

  return exceptionOccurred ? -1 : 1;
}