virtual_camera2.cpp

Go to the documentation of this file.

#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 "LadybugCamera.h"
#include "LadybugVirtualCamera.h"
#include "omnicamera_msgs/GetVirtualCameraConfig.h"
#include "omnicamera_msgs/VirtualCameraConfig.h"

using std::string;

using nifti::ladybug::LadybugVirtualCamera;
using omnicamera_msgs::GetVirtualCameraConfig;
using omnicamera_msgs::VirtualCameraConfig;

namespace {
LadybugVirtualCamera virtualCamera;
VirtualCameraConfig virtualCameraConfig;
double imageUpdateInterval = 5.0;
long lastImageUpdate = 0l;
sensor_msgs::CvBridge bridge;
bool viewChanged = false;

// For measuring cpu time.
struct tms t1, t2;
long r1 = 0l, r2 = 0l;

bool
getVirtualCameraConfigService(GetVirtualCameraConfig::Request& request, GetVirtualCameraConfig::Response& response);
void virtualCameraConfigCallback(const VirtualCameraConfig::ConstPtr& msg);
void imageCallback(const sensor_msgs::ImageConstPtr &msg);
void startDuration();
void logDuration(char* message);
}

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

  ros::init(argc, argv, "virtual_camera2");
  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.\n");
    printf("Usage:   virtual_camera_filter <mesh file path> <alphamask file prefix>\n");
    printf("Example: virtual_camera_filter mesh.txt alphamask\n");
    return -1;
  } else {
    meshFile = string(argv[1]);
    alphamaskPrefix = string(argv[2]);
    glutInit(&argc, argv);
  }

  // Setup virtual camera config topic and service.
  ros::Subscriber virtualCameraConfigSubscriber;
  ros::ServiceServer getVirtualCameraConfigServer;
  virtualCameraConfigSubscriber = nodeHandle.subscribe("virtual_camera/config", 1, virtualCameraConfigCallback);
  getVirtualCameraConfigServer
      = nodeHandle.advertiseService("virtual_camera/get_config", getVirtualCameraConfigService);

  image_transport::ImageTransport it(nodeHandle);
  // Advertise virtual camera view.
  image_transport::Publisher virtualCameraPub = it.advertise("virtual_camera/image", 1);
  // Subscribe to composite images to be used in rendering virtual camera view.
  image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);

  try {
    uint32_t seq = 0;
    virtualCamera.initialize(true);
    virtualCamera.loadMesh(meshFile);
    virtualCamera.loadAlphamask(alphamaskPrefix);

    while (nodeHandle.ok()) {
      // No synchronization needed since only one thread is used
      // both for callback processing and virtual camera rendering.
      // Process all pending callbacks.
      ros::spinOnce();

      if (viewChanged && virtualCameraPub.getNumSubscribers() > 0) {
        ROS_INFO("Rendering virtual camera view...");
        cv::Mat virtualCameraView;
        startDuration();
        virtualCamera.renderView(virtualCameraView);
        logDuration("Rendering view");

//        IplImage iplImage = virtualCameraView;
        IplImage iplImage = virtualCameraView.operator _IplImage();
//        sensor_msgs::ImagePtr msg = bridge.cvToImgMsg(&iplImage, "bgr8");
        sensor_msgs::ImagePtr msg = bridge.cvToImgMsg(&iplImage);
        msg->header.frame_id = "tfladylink";
        msg->header.seq = seq++;
        msg->header.stamp = ros::Time::now();

        startDuration();
        virtualCameraPub.publish(msg);
        logDuration("Publishing images");

        viewChanged = false;
      }
    }
  } catch (std::runtime_error& e) {
    ROS_ERROR("Runtime error: %s", e.what());
    return -1;
  } catch (...) {
    ROS_ERROR("Unexpected exception occurred.");
    return -1;
  }

  return 0;
}

namespace {

void imageCallback(const sensor_msgs::ImageConstPtr &msg) {
  struct tms t;
  long r;
  r = times(&t);
  double sinceLastUpdate = (double) (r - lastImageUpdate) / HZ;
  if (sinceLastUpdate >= imageUpdateInterval) {
    try {
      IplImage* iplImg = bridge.imgMsgToCv(msg, "bgr8");
//      IplImage* iplImg = bridge.imgMsgToCv(msg);
//      IplImage* iplImg = sensor_msgs::CvBridge::imgMsgToCv(msg);

      cv::Mat composite(iplImg);

      startDuration();
      virtualCamera.setImages(composite);
      logDuration("Updating virtual camera images");
    } catch (sensor_msgs::CvBridgeException& e) {
      ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
    lastImageUpdate = r;
    viewChanged = true;
  }
}

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.

  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;

  return true;
}

void virtualCameraConfigCallback(const VirtualCameraConfig::ConstPtr& msg) {
  ROS_DEBUG("virtualCameraConfigCallback called");

  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;

  viewChanged = true;
}

void startDuration() {
  r1 = times(&t1);
}

void logDuration(char* message) {
  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, realTime, userTime, systemTime);
}

}