ueyecamera.cpp

Go to the documentation of this file.

#include <uEye.h>
#include <iostream>
#include <boost/format.hpp>
#include <ros/ros.h>
#include <ros/console.h>
#include <image_transport/image_transport.h>
using namespace std;
using boost::format;


int main(int argc, char*argv[]){
  ros::init(argc, argv, "ueyecamera");
  ros::NodeHandle nh("~");
  int cameraid;
  int img_width=752, img_height=480, img_bpp=8, img_step, img_data_size, rate, img_left, img_top;

  sensor_msgs::Image img_msg;
  sensor_msgs::CameraInfo caminfo_msg;

  XmlRpc::XmlRpcValue double_list;

  std::string file_str;

  nh.param<int>("cameraid", cameraid,0);
  nh.param<int>("width", img_width,752);
  nh.param<int>("height", img_height,480);
  nh.param<int>("left", img_left,-1);
  nh.param<int>("top", img_top,-1);
  nh.param<int>("rate", rate ,10);
  nh.param("camera_frame_id", img_msg.header.frame_id, std::string("head_camera"));
  nh.param("file_str",file_str,std::string(""));        

  nh.getParam("K", double_list);
  if ((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) &&
     (double_list.size() == 9)) {
     for (int i=0; i<9; i++) {
         ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
         caminfo_msg.K[i] = double_list[i];
     }
  }

  nh.getParam("D", double_list);

      if ((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray)) {
        caminfo_msg.D.resize(double_list.size());
        for (int i=0; i<double_list.size(); i++) {
          ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
          caminfo_msg.D[i] = double_list[i];
        }
      }

  nh.getParam("R", double_list);

      if ((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) &&
          (double_list.size() == 9)) {
        for (int i=0; i<9; i++) {
          ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
          caminfo_msg.R[i] = double_list[i];
        }
      }

  nh.getParam("P", double_list);

      if ((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) &&
          (double_list.size() == 12)) {
        for (int i=0; i<12; i++) {
          ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
          caminfo_msg.P[i] = double_list[i];
        }
      }

  cerr<<"Trying to open camera: "<<cameraid<<endl
      <<"   width: "<<img_width<<endl
      <<"  height: "<<img_height<<endl
      <<"    rate: "<<rate<<endl;

  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("image", 1);
  ros::Publisher pub_info = nh.advertise<sensor_msgs::CameraInfo>("camera_info",1);

  HIDS hCam = cameraid;
  char* imgMem;
  int memId;
  if(is_InitCamera (&hCam, NULL)!= IS_SUCCESS){
    ROS_FATAL("Error Initializing camera.");
    return 0;
  }
  
  SENSORINFO sensor_info;
  is_GetSensorInfo (hCam, &sensor_info);
  cerr<<"Sensor info: "<<endl
      <<"  model: " <<sensor_info.strSensorName<<endl
      <<"  max width: "<<sensor_info.nMaxWidth<<endl
      <<"  max height: "<<sensor_info.nMaxHeight<<endl;
    
  if(img_width< sensor_info.nMaxWidth && img_left == -1){
    cerr<<"Left not specified. Assuiming centering the sensor"<<endl;
    img_left = (sensor_info.nMaxWidth - img_width)/2;
  }
  if(img_height< sensor_info.nMaxHeight && img_top == -1){
    cerr<<"Top not specified. Assuiming centering the sensor"<<endl;
    img_top = (sensor_info.nMaxHeight - img_height)/2;
  }


  is_AllocImageMem(hCam, img_width, img_height, img_bpp, &imgMem, &memId);
  is_SetImageMem (hCam, imgMem, memId);

  is_SetDisplayMode (hCam, IS_SET_DM_DIB);
  is_SetColorMode (hCam, IS_CM_BGR8_PACKED);
  //  is_SetColorMode (hCam, IS_CM_MONO8);
  // is_SetImageSize (hCam, img_width, img_height);
  IS_RECT rectAOI;
  rectAOI.s32X = img_left;
  rectAOI.s32Y = img_top;
  rectAOI.s32Width = img_width;
  rectAOI.s32Height = img_height; 
  // DEPRECATED: is_SetAOI(hCam, IS_SET_IMAGE_AOI, &img_left, &img_top, &img_width, &img_height);
  is_AOI(hCam, IS_AOI_IMAGE_SET_AOI, (void*)&rectAOI, sizeof(rectAOI));

  double enable = 1;
  //enable automatic gain
  is_SetAutoParameter (hCam, IS_SET_ENABLE_AUTO_GAIN, &enable, 0);
  is_SetAutoParameter (hCam, IS_SET_ENABLE_AUTO_WHITEBALANCE, &enable, 0);
  is_SetAutoParameter (hCam, IS_SET_ENABLE_AUTO_FRAMERATE, &enable, 0);
  is_SetAutoParameter (hCam, IS_SET_ENABLE_AUTO_SHUTTER, &enable, 0);
  
  ros::Rate loop_rate(rate);


  //is_GetImageMem(hCam, &data);
  is_GetImageMemPitch(hCam, &img_step);
  //img_msg.encoding="bgr8";
  img_msg.encoding="mono8";
  img_msg.height=img_height;
  img_msg.width=img_width;
  img_msg.step=img_step;
  img_msg.is_bigendian=0;
  img_data_size=img_height*img_step;
  img_msg.data.resize(img_data_size); 
  caminfo_msg.header.frame_id = img_msg.header.frame_id;
  caminfo_msg.height = img_height;
  caminfo_msg.width = img_width;

  if(!file_str.empty()){
        ROS_INFO("loading custom camera parameters:");
        ROS_INFO(file_str.c_str());
     // DEPRECATED: is_LoadParameters(hCam, file_str.c_str());  
        //is_ParameterSet(hCam, IS_PARAMETERSET_CMD_LOAD_FILE, (void *)file_str.c_str(), NULL);
        is_ParameterSet(hCam, IS_PARAMETERSET_CMD_LOAD_EEPROM, NULL, NULL);
        //is_ParameterSet(hCam, IS_PARAMETERSET_CMD_SAVE_EEPROM, NULL, NULL);

        //int pnCol,pnColMode;
        //is_GetColorDepth(hCam, &pnCol, &pnColMode);
        //if (pnCol==1) img_msg.encoding="mono8";
        //else img_msg.encoding="bgr8";
        }

  while (nh.ok()) {

    if(is_FreezeVideo(hCam, IS_WAIT) == IS_SUCCESS){
      std::copy((char*) imgMem, ((char*)imgMem) + img_data_size, img_msg.data.begin());
      img_msg.header.stamp=ros::Time::now();
      caminfo_msg.header.stamp=img_msg.header.stamp;
      pub.publish(img_msg);
      pub_info.publish(caminfo_msg);

  }
    else{
      ROS_INFO("Error freezing video");
    }


    ros::spinOnce();
    loop_rate.sleep();
  }
  is_ExitCamera(hCam);
  return 0;
}