jpeg_subscriber.cpp

Go to the documentation of this file.

/*
 * jpeg_subscriber.cpp
 *
 *  Created on: Mar 18, 2011
 *      Author: petrito1
 */

#include <math.h>

#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <ros/time.h>
#include <sensor_msgs/CompressedImage.h>
#include <cv_bridge/CvBridge.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;
using nifti::dc1394::LadybugCamera;

namespace {
const std::string jpegIn = "in";
const std::string rgbOut = "out";
}

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

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

  image_transport::ImageTransport it(nodeHandle);
  image_transport::Publisher pub = it.advertise(rgbOut, 2);

  try {

    CameraContext context;

    LadybugCamera camera = context.getLadybugCamera(10095209);
    camera.resetBus();
    camera.reset();
    camera.setOperationMode(DC1394_OPERATION_MODE_1394B);
    camera.setIsoSpeed(DC1394_ISO_SPEED_800);
//    dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_0;
    dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_6; // JPEG
    camera.setVideoMode(videoMode);
    camera.setSelectedImages(LadybugCamera::ALL_CAMERAS);
    camera.setFormat7Roi(videoMode);
    camera.setBayerMethod(DC1394_BAYER_METHOD_NEAREST);
    camera.setupCapture();
    camera.startVideoTransmission();

    sensor_msgs::CvBridge bridge;

    uint32_t seq = 0;
    while (nodeHandle.ok()) {
      cv::Mat frame;
      uint64_t timestamp;
      camera.captureColorMat(frame, timestamp, true);

      // Publish decoded image.
      if (pub.getNumSubscribers() > 0) {
        IplImage iplImage = frame;
        sensor_msgs::ImagePtr msg = bridge.cvToImgMsg(&iplImage, "bgr8");
        msg->header.frame_id = "tfladylink";
        msg->header.seq = seq;
        msg->header.stamp = ros::Time::now();
        pub.publish(msg);
      }
    }

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

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

  return 0;
}