leaf_fitting_alg_node.cpp

Go to the documentation of this file.

#include "leaf_fitting_alg_node.h"

LeafFittingAlgNode::LeafFittingAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<LeafFittingAlgorithm>()
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  this->output_image_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Image>("output_image", 100);
  
  // [init subscribers]
  
  // [init services]
  get_confidence_leaf_fitting_srv_ = public_node_handle_.advertiseService("get_confidence_leaf_fitting_srv", &LeafFittingAlgNode::get_confidence_leaf_fitting_srv_callback, this);

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

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

void LeafFittingAlgNode::mainNodeThread(void)
{


  // [fill msg structures]
  //this->Image_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]


  // [publish messages]
}

/*  [subscriber callbacks] */

/*  [service callbacks] */
bool LeafFittingAlgNode::get_confidence_leaf_fitting_srv_callback(iri_leaf_fitting::GetConfidenceLeafFitting::Request &req, iri_leaf_fitting::GetConfidenceLeafFitting::Response &res)
{
  ROS_INFO("LeafSegmentationAlgNode::get_clustered_image_srv_callback: New Service Request Received");

  // Memory allocation
  cluster_labels_ = (int**)malloc(sizeof(int*)*200);
  for (int k=0; k<200; k++)
    cluster_labels_[k] = (int*)malloc(sizeof(int)*200);  
 
  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock(); 
 
  // fill in the cluster_label array with the msg data
  int ii=0;
  for (int rr=0; rr < 200; ++rr)
    for (int cc=0; cc < 200; ++cc, ++ii) 
        cluster_labels_[rr][cc] = req.cluster_labels.data[ii];

  num_ccs_fin_ = req.num_clusters.data;

  //unlock previously blocked shared variables 
  this->alg_.unlock(); 

  ROS_INFO("1)Num of clusters: %d\n", num_ccs_fin_);

  //TODO: This is too hardcoded, try to get the images dynamically
  image<rgb> *leaf_model = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/iri_perception/iri_leaf_fitting/model/model_A.ppm");
  image<rgb> *leaf_model_area = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/iri_perception/iri_leaf_fitting/model/model_A_bound.ppm");
  
  // Main Algorithm
  double *fit_score_list;
  image<rgb> *leaf_confidence = leaf_fitting(cluster_labels_, leaf_model, leaf_model_area, num_ccs_fin_, fit_score_list);
  
  res.fit_score_list.data.resize(num_ccs_fin_);
  for (int ii=0; ii < num_ccs_fin_; ++ii)
    res.fit_score_list.data[ii] = fit_score_list[ii];

  // Create segmented rgb image for publishing it afterwards
  //TODO: This is too hardcoded, try to get the matrix dimensions from somewhere
  cv::Mat image(200, 200, CV_8UC3);
  cv::Mat_<cv::Vec3b>::iterator img_iter = image.begin<cv::Vec3b>();
  for (int rr = 0; rr < image.rows; ++rr) {                         
    for (int cc = 0; cc < image.cols; ++cc, ++img_iter ) {         
      (*img_iter)[0] = leaf_confidence->access[rr][cc].r;  
      (*img_iter)[1] = leaf_confidence->access[rr][cc].g; 
      (*img_iter)[2] = leaf_confidence->access[rr][cc].b;    
    }                                                                 
  }                                                                   
  imwrite("/home/sfoix/temp/input_cv.jpg", image);

  cv_ptr_ = boost::make_shared<cv_bridge::CvImage>();
  //TODO: This is too hardcoded, try to see if it can be done more elegantly
  cv_ptr_->header.seq = 1;
  cv_ptr_->header.stamp = ros::Time();
  cv_ptr_->header.frame_id = "camera_frame";
  cv_ptr_->encoding = sensor_msgs::image_encodings::RGB8;
  cv_ptr_->image = image;

  this->output_image_publisher_.publish(cv_ptr_->toImageMsg());

  return 1;
}


/*  [action callbacks] */

/*  [action requests] */

void LeafFittingAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();

  this->alg_.unlock();
}

void LeafFittingAlgNode::addNodeDiagnostics(void)
{
}

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