omnicamera.cpp

Go to the documentation of this file.

/*
 * omnicamera.cpp
 *
 *  Created on: Jul 29, 2010
 *      Author: petrito1@cmp.felk.cvut.cz
 *
 * ROS node providing various image topics from Ladybug 3 camera.
 *
 * Single-lens virtual camera (e.g. used with virtual_camera_display node):
 * virtual_camera/image - publishing virtual camera images
 * virtual_camera/config - subscribed for virtual camera configuration
 * virtual_camera/get_config - service for getting current configuration
 *
 * Composite color image:
 * camera/image - publishing composite color images--6 landscape images sta
 * camera0/image - publishing color images from individual sensors, as portrait images.
 * camera1/image
 * camera2/image
 * camera3/image
 * camera4/image
 * camera5/image
 *
 * TODO: Publish raw images, color images, stitched panos etc. according
 * to registered subscribers.
 */

#include <cv_bridge/CvBridge.h>
#include <GL/glew.h> // glew.h must be included before gl.h
#include <GL/gl.h>
#include <GL/glut.h>
#include <image_transport/image_transport.h>
#include <math.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <ros/ros.h>
#include <sys/param.h>
#include <sys/times.h>

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

#include "omnicamera_msgs/GetLadybugCameraConfig.h"
#include "omnicamera_msgs/GetVirtualCameraConfig.h"
#include "omnicamera_msgs/VirtualCameraConfig.h"
#include "omnicamera_msgs/LadybugCameraConfig.h"

using std::string;

using nifti::dc1394::Camera;
using nifti::dc1394::CameraContext;
using nifti::dc1394::CameraException;
using nifti::dc1394::LadybugCamera;
using nifti::ladybug::LadybugVirtualCamera;

using omnicamera_msgs::GetVirtualCameraConfig;
using omnicamera_msgs::GetLadybugCameraConfig;
using omnicamera_msgs::LadybugCameraConfig;
using omnicamera_msgs::VirtualCameraConfig;

namespace {
LadybugVirtualCamera virtualCamera;
bool virtualCameraEnabled = false;
LadybugCamera camera;
LadybugCameraConfig ladybugCameraConfig;
VirtualCameraConfig virtualCameraConfig;
sensor_msgs::CvBridge bridge;

double imageUpdateInterval = 5.0;

pthread_mutex_t virtualCameraMutex;
void lockVirtualCamera();
void unlockVirtualCamera();

bool
getLadybugCameraConfigService(GetLadybugCameraConfig::Request& request, GetLadybugCameraConfig::Response& response);
void ladybugCameraConfigCallback(const LadybugCameraConfig::ConstPtr& msg);

bool
getVirtualCameraConfigService(GetVirtualCameraConfig::Request& request, GetVirtualCameraConfig::Response& response);
void virtualCameraConfigCallback(const VirtualCameraConfig::ConstPtr& msg);

void logDuration(string message, tms t1, long r1);
}

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

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

  for (int i = 0; i < argc; i++) {
          printf("%d: %s\n", i, argv[i]);
  }

  // Check and parse command-line arguments.
  string meshFile;
  string alphamaskPrefix;
  if (argc < 3) {
    printf("Not enough arguments. Virtual camera is not used.\n");
    printf("Usage:   omnicamera_node <mesh file path> <alphamask file prefix>\n");
    printf("Example: omnicamera_node mesh.txt alphamask\n");
  } else {
    virtualCameraEnabled = true;
    meshFile = string(argv[1]);
    alphamaskPrefix = string(argv[2]);
    glutInit(&argc, argv);
  }

  pthread_mutex_init(&virtualCameraMutex, NULL);

  ros::Subscriber virtualCameraConfigSubscriber;
  ros::ServiceServer getVirtualCameraConfigServer;
  if (virtualCameraEnabled) {
    virtualCameraConfigSubscriber = nodeHandle.subscribe("virtual_camera/config", 1,
        virtualCameraConfigCallback);
    getVirtualCameraConfigServer = nodeHandle.advertiseService("virtual_camera/get_config",
        getVirtualCameraConfigService);
  }

  ros::Subscriber ladybugCameraConfigSubscriber = nodeHandle.subscribe("ladybug_camera/config", 1,
      ladybugCameraConfigCallback);
  ros::ServiceServer getLadybugCameraConfigServer = nodeHandle.advertiseService("ladybug_camera/get_config",
      getLadybugCameraConfigService);

  image_transport::ImageTransport it(nodeHandle);
  image_transport::Publisher virtualCameraPub = it.advertise("virtual_camera/image", 1);

  image_transport::Publisher cameraPub = it.advertise("camera/image", 1);

  image_transport::Publisher sensorPubs[LadybugCamera::NUM_CAMERAS] = { it.advertise("camera0/image", 1), it.advertise(
      "camera1/image", 1), it.advertise("camera2/image", 1), it.advertise("camera3/image", 1), it.advertise(
      "camera4/image", 1), it.advertise("camera5/image", 1) };

  ros::Publisher cameraInfoPubs[LadybugCamera::NUM_CAMERAS] = {
      nodeHandle.advertise<sensor_msgs::CameraInfo>("camera0/camera_info", 1),
      nodeHandle.advertise<sensor_msgs::CameraInfo>("camera1/camera_info", 1),
      nodeHandle.advertise<sensor_msgs::CameraInfo>("camera2/camera_info", 1),
      nodeHandle.advertise<sensor_msgs::CameraInfo>("camera3/camera_info", 1),
      nodeHandle.advertise<sensor_msgs::CameraInfo>("camera4/camera_info", 1),
      nodeHandle.advertise<sensor_msgs::CameraInfo>("camera5/camera_info", 1) };

  string cameraFrameIds[LadybugCamera::NUM_CAMERAS] = { "tflbcamera0", "tflbcamera1", "tflbcamera2", "tflbcamera3", "tflbcamera4", "tflbcamera5" };

  try {

    if (virtualCameraEnabled) {
      virtualCamera.initialize(true);
      virtualCamera.loadMesh(meshFile);
      virtualCamera.loadAlphamask(alphamaskPrefix);
    }

    CameraContext context;

    // TODO: Move serial number to ROS param.
    // Use default GUID to use the first camera in the list.
    camera = context.getLadybugCamera(0);
    camera.resetBus();
    sleep(3);
    camera.reset();

    //    camera.setup();
    camera.setOperationMode(DC1394_OPERATION_MODE_LEGACY);
    camera.setIsoSpeed(DC1394_ISO_SPEED_400);
//    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_2;
    camera.setVideoMode(videoMode);
    camera.setSelectedImages(LadybugCamera::ALL_CAMERAS);
    // Workaround for new firewire stack until it can handle larger packet size.
    // According to https://ieee1394.wiki.kernel.org/index.php/Release_Notes
    // the max. size it can handle pre 2.6.38 is even smaller (4080).
//    camera.setFormat7Roi(videoMode, 8192);
    camera.setFormat7Roi(videoMode, 4096);
    camera.setBayerMethod(DC1394_BAYER_METHOD_SIMPLE);
//    camera.setBayerMethod(DC1394_BAYER_METHOD_HQLINEAR);

    camera.setupCapture();
    camera.startVideoTransmission();

    struct tms t2;
    long r1 = 0l;
    long r2 = 0l;
    
    uint32_t seq = 0;
    ros::Rate r(5);
    while (nodeHandle.ok()) {
      ros::spinOnce();

      cv::Mat compositeImage;
      uint64_t timestamp;
      camera.captureColorMat(compositeImage, timestamp, true);
      
      // Fix aspect ratio to 4x3 for half-height images etc.
      if (videoMode == DC1394_VIDEO_MODE_FORMAT7_2 || videoMode == DC1394_VIDEO_MODE_FORMAT7_3) {
        cv::Mat dst;
        cv::resize(compositeImage, dst, cv::Size(compositeImage.cols / 2, compositeImage.rows), 0, 0, CV_INTER_NN);
        compositeImage = dst;
      }

      ros::Time imageTime = ros::Time(static_cast<double>(timestamp) / 1000000.0);

      if (cameraPub.getNumSubscribers() > 0) {
        IplImage iplImage = compositeImage;
        sensor_msgs::ImagePtr msg = bridge.cvToImgMsg(&iplImage, "bgr8");
//        msg->header.frame_id = "/tfladylink";
        msg->header.frame_id = "/omni_frame";
        msg->header.seq = seq;
        msg->header.stamp = imageTime;
        cameraPub.publish(msg);
      }

      int iImage = -1;
      for (int i = 0; i < LadybugCamera::NUM_CAMERAS; i++) {
        if (!camera.areImagesSelected(LadybugCamera::CAMERAS[i])) {
          continue;
        }
        iImage++;
        if (sensorPubs[i].getNumSubscribers() > 0) {
          int imageHeight = compositeImage.rows / camera.getNumSelectedImages();

          // Publish portrait images.
          cv::Mat image = compositeImage(cv::Rect(0, iImage * imageHeight, compositeImage.cols, imageHeight)).t();
          // Flip the image horizontally.
          cv::flip(image, image, 1);

          IplImage iplSensorImage = image;
          sensor_msgs::ImagePtr msg = bridge.cvToImgMsg(&iplSensorImage, "bgr8");
          msg->header.frame_id = cameraFrameIds[i];
          msg->header.seq = seq;
          msg->header.stamp = imageTime;

          sensorPubs[i].publish(msg);

          sensor_msgs::CameraInfo cameraInfoMsg;
          cameraInfoMsg.header.frame_id = cameraFrameIds[i];
          cameraInfoMsg.height = image.rows;
          cameraInfoMsg.width = image.cols;
          cameraInfoPubs[i].publish(cameraInfoMsg);
        }
      }

      if (virtualCameraEnabled && virtualCameraPub.getNumSubscribers() > 0) {
        r2 = times(&t2);
        if ((double) (r2 - r1) / HZ >= imageUpdateInterval) {
          r1 = r2;
          virtualCamera.setImages(compositeImage);
          logDuration("Updating virtual camera images", t2, r2);
        }

        r2 = times(&t2);
        cv::Mat virtualView;
        virtualCamera.renderView(virtualView);
        logDuration("Rendering view", t2, r2);

        r2 = times(&t2);
        IplImage iplImage = virtualView.operator _IplImage();
//        sensor_msgs::ImagePtr msg = sensor_msgs::CvBridge::cvToImgMsg(&iplImage);
        sensor_msgs::ImagePtr msg = bridge.cvToImgMsg(&iplImage, "bgr8");
//        sensor_msgs::ImagePtr msg = bridge.cvToImgMsg(&iplImage);
//        msg->header.frame_id = "/tfladylink";
        msg->header.frame_id = "/omni_frame";
        msg->header.seq = seq;
        msg->header.stamp = ros::Time::now();
        virtualCameraPub.publish(msg);
        logDuration("Publishing images", t2, r2);
      }

      seq++;
      r.sleep();
    }

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

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

  return 0;
}

namespace {
void lockVirtualCamera() {
  pthread_mutex_lock(&virtualCameraMutex);
}
void unlockVirtualCamera() {
  pthread_mutex_unlock(&virtualCameraMutex);
}

bool getVirtualCameraConfigService(GetVirtualCameraConfig::Request& request, GetVirtualCameraConfig::Response& response) {
  ROS_DEBUG("getVirtualCameraConfigService called");
  // There is nothing interesting in the request...
  // Fill the response with current values. Prevent returning inconsistent state.
  lockVirtualCamera();

  int viewportWidth, viewportHeight;
  virtualCamera.getViewport(&viewportWidth, &viewportHeight);
  response.config.viewportWidth = viewportWidth;
  response.config.viewportHeight = viewportHeight;
  response.config.pan = virtualCamera.getPan();
  response.config.tilt = virtualCamera.getTilt();
  response.config.horizontalFov = virtualCamera.getHorizontalFov();
  response.config.verticalFov = virtualCamera.getVerticalFov();

  response.config.imageUpdateInterval = imageUpdateInterval;

  unlockVirtualCamera();

  return true;
}

void virtualCameraConfigCallback(const VirtualCameraConfig::ConstPtr& msg) {
  ROS_DEBUG("virtualCameraConfigCallback called");
  // Prevent concurrent modification.
  lockVirtualCamera();

  virtualCamera.setViewport(msg->viewportWidth, msg->viewportHeight);
  virtualCamera.setPan(msg->pan);
  virtualCamera.setTilt(msg->tilt);
  virtualCamera.setHorizontalFov(msg->horizontalFov);
  virtualCamera.setVerticalFov(msg->verticalFov);

  imageUpdateInterval = msg->imageUpdateInterval;

  unlockVirtualCamera();
}

void ladybugCameraConfigCallback(const LadybugCameraConfig::ConstPtr& msg) {
  ROS_DEBUG("ladybugCameraConfigCallback called");

  // TODO: Lock camera here?
  //  camera.lockCapture();
  camera.stopCapture();
  camera.stopVideoTransmission();

  ladybugCameraConfig = *msg;
  camera.setSelectedImages(msg->imagesSelected);

  camera.setupCapture();
  camera.startVideoTransmission();
  //  camera.unlockCapture();
}

bool getLadybugCameraConfigService(GetLadybugCameraConfig::Request& request, GetLadybugCameraConfig::Response& response) {
  ROS_DEBUG("getLadybugCameraConfigService called");
  // There is nothing interesting in the request...
  // Fill the response with current values. Prevent returning inconsistent state.
  // TODO: Lock camera?

  ladybugCameraConfig.imagesSelected = camera.getSelectedImages();
  response.config = ladybugCameraConfig;
  return true;
}

void logDuration(string message, tms t1, long r1) {
  struct tms t2;
  long r2;
  r2 = times(&t2);

  float userTime = ((float) (t2.tms_utime - t1.tms_utime)) / HZ;
  float systemTime = ((float) (t2.tms_stime - t1.tms_stime)) / HZ;
  float realTime = ((float) (r2 - r1)) / HZ;

  std::string format = "%s: real time: %1.2f, user time: %1.2f, system time: %1.2f";
  ROS_DEBUG(format.data(), message.data(), realTime, userTime, systemTime);
  //  printf(format.data(), message, realTime, userTime, systemTime);
}

}