image_reader_alg_node.cpp

Go to the documentation of this file.

#include "image_reader_alg_node.h"

ImageReaderAlgNode::ImageReaderAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<ImageReaderAlgorithm>(),
  it_(this->public_node_handle_),
  iit_database(0),
  tf_listener_(ros::Duration(30.f)), list_pointer(0)
{
  //init class attributes if necessary
  this->loop_rate_ = 10;//in [Hz]

  // [init publishers]
  image_pub_ = it_.advertiseCamera("image_raw", 100);
 
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
//  init_sensor_info();
}

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

void ImageReaderAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  if (iit_database < 2157) //database length
  update_msgs();  
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
  sensor_msgs::Image image_;
  this->img_msg_.toImageMsg( image_ );
  this->image_pub_.publish( image_,  this->CameraInfo_msg_ );

}

void ImageReaderAlgNode::init_sensor_info()
{
        //Image
        img_msg_.header.frame_id = "/camera";

        //      Camera Info -----------------------------------------------------
        CameraInfo_msg_.height = 960;//480 ;
        CameraInfo_msg_.width = 1280;//640;
        CameraInfo_msg_.distortion_model = "plumb_bob";

        CameraInfo_msg_.D.clear();
        CameraInfo_msg_.D.push_back(0);
        CameraInfo_msg_.D.push_back(0);
        CameraInfo_msg_.D.push_back(0);
        CameraInfo_msg_.D.push_back(0);
        CameraInfo_msg_.D.push_back(0);

        CameraInfo_msg_.K[0] =  0;
        CameraInfo_msg_.K[2] = 0;
        CameraInfo_msg_.K[4] =  0;
        CameraInfo_msg_.K[5] = 0;
        CameraInfo_msg_.K[8] = 0;

// float64[9] CameraInfo_msg_.R
        CameraInfo_msg_.R[0] =  0;
        CameraInfo_msg_.R[4] =  0;
        CameraInfo_msg_.R[8] =  0;

// float64[12] CameraInfo_msg_.P := K *[R | t]
        CameraInfo_msg_.P[0] =  0*0;
        CameraInfo_msg_.P[2] = 0*0;
        CameraInfo_msg_.P[5] =  0*0;
        CameraInfo_msg_.P[6] = 0*0;
        CameraInfo_msg_.P[10] = 1;

        CameraInfo_msg_.header.frame_id = "/camera";
    cam_model_.fromCameraInfo(CameraInfo_msg_);
}

void ImageReaderAlgNode::update_msgs()
{
  cv::Mat outIm;
  ros::Time now = ros::Time::now();
  iit_database ++;
  //    Header
  CameraInfo_msg_.header.seq   = iit_database;
  CameraInfo_msg_.header.stamp = now;
  img_msg_.header.seq          = iit_database;
  img_msg_.header.stamp        = now;

  //Image --------------------------------------------------------
  std::string fileString = path + "/list.lst";
  FILE * file_out;
  file_out = fopen (  fileString.c_str() , "r");
  fseek(file_out , list_pointer*(iit_database -1) , SEEK_SET );
  char file_name[32];
  if (fscanf(file_out, "%s\n" , file_name) != EOF) { 
    std::string string_file_name = file_name;
//  ROS_INFO("%s",string_file_name.c_str()); 
    fileString = path + string_file_name; //list.lst -> listed all file names...
    outIm = cv::imread( fileString.c_str(), 1 );
    img_msg_.encoding = "bgr8";
    img_msg_.image    = outIm;
  }
  
  if (list_pointer == 0)
    list_pointer = strlen(file_name) + 1;
  fclose(file_out);
}



/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void ImageReaderAlgNode::node_config_update(Config &config, uint32_t level)
{
  ROS_DEBUG("MyDatabasePublAlgNode::node_config_update");
  this->alg_.lock();
        iit_database = config.sequence_iterator;
        loop_rate_   = config.frame_rate;
        path   = config.images_path;

  this->alg_.unlock();
}

void ImageReaderAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<ImageReaderAlgNode>(argc, argv, "image_reader_alg_node");
}