virtual_camera.cpp

Go to the documentation of this file.

#include <GL/glew.h>
#include <GL/gl.h>
#include <GL/glut.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sys/param.h>
#include <sys/times.h>

#include <cv_bridge/CvBridge.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>

#include "LadybugVirtualCamera.h"

#define TEXTURE_UPDATE_INTERVAL 5.0

using nifti::ladybug::LadybugVirtualCamera;

namespace {
std::string nodeName = "virtual_camera";
LadybugVirtualCamera virtualCamera;

// Time vars for tracking image update frequency.
struct tms t1, t2;
long r1 = 0l;
long r2 = 0l;

// Mouse button state and coords where button was pressed.
int mouseState = GLUT_UP;
int mouseX, mouseY;

// ROS context.
ros::NodeHandle* nodeHandlePtr;
image_transport::Subscriber sub;
sensor_msgs::CvBridge bridge;

// Images to show.
pthread_mutex_t imageMutex = PTHREAD_MUTEX_INITIALIZER;
void lockImage() {
  pthread_mutex_lock(&imageMutex);
}
void unlockImage() {
  pthread_mutex_unlock(&imageMutex);
}

void idle(void) {
  glutPostRedisplay();
}

void resizeCallback(int w, int h) {
  ROS_DEBUG("resizeCallback: Resizing to %dx%d.", w, h);

  virtualCamera.setViewport(w, h);
  virtualCamera.setHorizontalFov(60.0);
  virtualCamera.setVerticalFov(60.0 * w / h);

  virtualCamera.updateViewport();
  virtualCamera.updateProjection();
}

void mouseCallback(int button, int state, int x, int y) {
  ROS_DEBUG("mouseCallback: x: %d, y: %d.", x, y);

  mouseState = state;
  if (mouseState == GLUT_DOWN) {
    mouseX = x;
    mouseY = y;
  }
}

void motionCallback(int x, int y) {
  ROS_DEBUG("motionCallback: x: %d, y: %d.", x, y);

  if (mouseState == GLUT_DOWN) {
    virtualCamera.setPan(virtualCamera.getPan() + (double) (x - mouseX));
    virtualCamera.setTilt(virtualCamera.getTilt() + (double) (y - mouseY));
    mouseX = x;
    mouseY = y;
  }
}

void displayCallback(void) {
  ROS_DEBUG("displayCallback: Rendering virtual camera view.");

  virtualCamera.clearBuffers();
  virtualCamera.updateModelView();

  //  lockImage();
  virtualCamera.render();
  //  unlockImage();

  glutSwapBuffers();
}

void imageSubscriberCallback(const sensor_msgs::ImageConstPtr &msg) {
  ROS_DEBUG("imageSubscriberCallback: Updating virtual camera images.");

  // Return if too soon from previous texture update.
  r2 = times(&t2);
  if ((float) (r2 - r1) / HZ < TEXTURE_UPDATE_INTERVAL) {
    ROS_DEBUG("imageSubscriberCallback: Skipping image message.");
    return;
  }
  t1 = t2;
  r1 = r2;

  // TODO: Lock image?
  //  lockImage();

  try {
    IplImage* iplImg = bridge.imgMsgToCv(msg, "bgr8");
    cv::Mat composite(iplImg);

    ROS_DEBUG("imageSubscriberCallback: Setting images.");
    printf("imageSubscriberCallback: Setting images.\n");
    virtualCamera.setImages(composite);

    // TODO: Free the image?

  } catch (sensor_msgs::CvBridgeException& e) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    //    unlockImage();
  }

  //  unlockImage();
}

void* subscriberThreadMain(void* ptr) {
  ROS_DEBUG("Starting ROS spin thread...");
  ros::spin();
  ROS_DEBUG("Stopping ROS spin thread...");
  return NULL;
}

}

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

  ros::init(argc, argv, nodeName);

  if (argc < 3) {
    printf("Usage: virtual_camera <mesh file> <alphamask prefix>");
    printf("Example: virtual_camera mesh.txt alphamask");
    return -1;
  }

  glutInit(&argc, argv);

  int viewportWidth, viewportHeight;
  virtualCamera.getViewport(&viewportWidth, &viewportHeight);
  glutInitWindowSize(viewportWidth, viewportHeight);
  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
  //  glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
  int glutWin = glutCreateWindow(nodeName.data());

  // On some systems a graphics context has to be initialized prior to
  // calling glewInit, e.g. in form of a GLUT window.
  //  GLenum glew = glewInit();
  //  if (GLEW_OK != glew) {
  //    ROS_ERROR("GLEW could not be initialized.");
  //    glutDestroyWindow(glutWin);
  //    return -1;
  //  }

  virtualCamera.initialize(false);
  virtualCamera.loadMesh(argv[1]);
  virtualCamera.loadAlphamask(argv[2]);

  glutReshapeFunc(resizeCallback);
  glutDisplayFunc(displayCallback);
  glutMouseFunc(mouseCallback);
  glutMotionFunc(motionCallback);
  glutIdleFunc(idle);

  ros::NodeHandle nodeHandle;
  nodeHandlePtr = &nodeHandle;

  image_transport::ImageTransport it(nodeHandle);
  sub = it.subscribe("image", 1, imageSubscriberCallback);

  // Start a thread for reading off published images.
  pthread_t subscriberThread;
  int subscriberThreadRet = pthread_create(&subscriberThread, NULL, subscriberThreadMain, (void*) NULL);

  glutMainLoop();
  glutDestroyWindow(glutWin);

  ros::shutdown();
  pthread_join(subscriberThread, NULL);

  return subscriberThreadRet;
}