ueye_camera_driver_node.cpp

Go to the documentation of this file.

#include "ueye_camera_driver_node.h"

using namespace std;

UeyeCameraDriverNode::UeyeCameraDriverNode(ros::NodeHandle &nh) : 
  iri_base_driver::IriBaseNodeDriver<UeyeCameraDriver>(nh)
{

  this->loop_rate_ = 100;//in [Hz]

  this->public_node_handle_.param<int>("cameraid", this->driver_.cameraid_,0);
  this->public_node_handle_.param<int>("param_mode", this->driver_.param_mode_,1);
  this->public_node_handle_.param<int>("width", this->driver_.img_width_,752);
  this->public_node_handle_.param<int>("height", this->driver_.img_height_,480);
  this->public_node_handle_.param<int>("left", this->driver_.img_left_,-1);
  this->public_node_handle_.param<int>("top", this->driver_.img_top_,-1);
  this->public_node_handle_.param<int>("pixel_clock", this->driver_.pixel_clock_,20);
  this->public_node_handle_.param<int>("fps", this->driver_.fps_,80);
  this->public_node_handle_.param<double>("exposure", this->driver_.exposure_,0.1);
  this->public_node_handle_.param<bool>("mirror_updown", this->driver_.mirror_updown_,false);
  this->public_node_handle_.param<bool>("mirror_leftright", this->driver_.mirror_leftright_,false);
  this->public_node_handle_.param<string>("camera_frame_id", this->driver_.frame_id_, string("ueye_camera"));
  this->public_node_handle_.param("file_str",this->driver_.file_str_,string(""));
  

  this->public_node_handle_.getParam("K", this->driver_.double_list_);
  if ((this->driver_.double_list_.getType() == XmlRpc::XmlRpcValue::TypeArray) && (this->driver_.double_list_.size() == 9)) {
     for (int i=0; i<9; i++) {
       ROS_ASSERT(this->driver_.double_list_[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
         this->CameraInfo_msg_.K[i] = this->driver_.double_list_[i];
     }
  }

  this->public_node_handle_.getParam("D", this->driver_.double_list_);
  if ((this->driver_.double_list_.getType() == XmlRpc::XmlRpcValue::TypeArray)) {
    this->CameraInfo_msg_.D.resize(this->driver_.double_list_.size());
    for (int i=0; i<this->driver_.double_list_.size(); i++) {
      ROS_ASSERT(this->driver_.double_list_[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
      this->CameraInfo_msg_.D[i] = this->driver_.double_list_[i];
    }
  }

  this->public_node_handle_.getParam("R", this->driver_.double_list_);
  if ((this->driver_.double_list_.getType() == XmlRpc::XmlRpcValue::TypeArray) && (this->driver_.double_list_.size() == 9)) {
    for (int i=0; i<9; i++) {
      ROS_ASSERT(this->driver_.double_list_[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
      this->CameraInfo_msg_.R[i] = this->driver_.double_list_[i];
    }
  }

  this->public_node_handle_.getParam("P", this->driver_.double_list_);
  if ((this->driver_.double_list_.getType() == XmlRpc::XmlRpcValue::TypeArray) && (this->driver_.double_list_.size() == 12)) {
    for (int i=0; i<12; i++) {
      ROS_ASSERT(this->driver_.double_list_[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
      this->CameraInfo_msg_.P[i] = this->driver_.double_list_[i];
    }
  }

  // [init publishers]
  this->camera_info_publisher_ = this->public_node_handle_.advertise<sensor_msgs::CameraInfo>("camera_info", 10);
  this->image_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Image>("image", 10);

  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

void UeyeCameraDriverNode::mainNodeThread(void)
{

  //lock access to driver if necessary
  this->driver_.lock();

  // [fill msg Header if necessary]
  this->Image_msg_.header.stamp=ros::Time::now();
  this->Image_msg_.header.frame_id=this->driver_.frame_id_;

  this->CameraInfo_msg_.header.stamp=this->Image_msg_.header.stamp;
  this->CameraInfo_msg_.header.frame_id = this->Image_msg_.header.frame_id;
  
  // [fill msg structures]
  if(this->driver_.ueye_cam_.params_.img_bpp==8)
    this->Image_msg_.encoding="mono8";
  else if(this->driver_.ueye_cam_.params_.img_bpp==24)
    this->Image_msg_.encoding="bgr8";
  this->Image_msg_.height=this->driver_.ueye_cam_.params_.img_height;
  this->Image_msg_.width=this->driver_.ueye_cam_.params_.img_width;
  this->Image_msg_.step=this->driver_.ueye_cam_.img_step_;
  this->Image_msg_.is_bigendian=0;
  this->driver_.img_data_size_=this->driver_.ueye_cam_.params_.img_height*this->driver_.ueye_cam_.img_step_;
  this->Image_msg_.data.resize(this->driver_.img_data_size_); 

  this->CameraInfo_msg_.height = this->driver_.ueye_cam_.params_.img_height;
  this->CameraInfo_msg_.width = this->driver_.ueye_cam_.params_.img_width;

  // Fill message
  if(this->driver_.get_image()){

    // get image
    this->Image_msg_.data = this->driver_.ueye_cam_.image_data_;

    // [publish messages]
    this->camera_info_publisher_.publish(this->CameraInfo_msg_);
    this->image_publisher_.publish(this->Image_msg_);
  }

  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  //unlock access to driver if previously blocked
  this->driver_.unlock();
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void UeyeCameraDriverNode::postNodeOpenHook(void)
{
}

void UeyeCameraDriverNode::addNodeDiagnostics(void)
{
}

void UeyeCameraDriverNode::addNodeOpenedTests(void)
{
}

void UeyeCameraDriverNode::addNodeStoppedTests(void)
{
}

void UeyeCameraDriverNode::addNodeRunningTests(void)
{
}

void UeyeCameraDriverNode::reconfigureNodeHook(int level)
{
}

UeyeCameraDriverNode::~UeyeCameraDriverNode(void)
{
  // [free dynamic memory]
}

/* main function */
int main(int argc,char *argv[])
{
  return driver_base::main<UeyeCameraDriverNode>(argc, argv, "ueye_camera_driver_node");
}