leaf_segmentation_alg_node.cpp

Go to the documentation of this file.

#include "leaf_segmentation_alg_node.h"
#include "segment-image.h"
#include "pnmfile.h"

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

  // [init publishers]
  output_image_publisher_ = public_node_handle_.advertise<sensor_msgs::Image>("output_image", 1);
  
  // [init subscribers]

  // [init services]
  get_clustered_image_srv_ = public_node_handle_.advertiseService("get_clustered_image_srv", &LeafSegmentationAlgNode::get_clustered_image_srv_callback, this);
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void LeafSegmentationAlgNode::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]

  // ROS_INFO("LeafSegmentationAlgNode::mainNodeThread"); 

  // [publish messages]
//  output_image_publisher_.publish(cv_ptr_->toImageMsg());
}

/*  [subscriber callbacks] */

/*  [service callbacks] */
bool LeafSegmentationAlgNode::get_clustered_image_srv_callback(iri_leaf_segmentation::GetClusteredImage::Request &req, iri_leaf_segmentation::GetClusteredImage::Response &res)
{ 
  ROS_INFO("LeafSegmentationAlgNode::get_clustered_image_srv_callback: New Service Request Received");
  // Convert from pcl_xyzrgb to image and point_cloud 2D images 
  image<rgb> *input;
  image<rgb> *point_cloud;

  pcl::fromROSMsg(req.point_cloud, pcl_xyzrgb_);

  input = new image<rgb>(pcl_xyzrgb_.height, pcl_xyzrgb_.width);
  point_cloud = new image<rgb>(pcl_xyzrgb_.height, pcl_xyzrgb_.width);
  pcl::PointCloud<pcl::PointXYZRGB>::iterator pcl_iter = pcl_xyzrgb_.begin();
  for (int rr = 0; rr < pcl_xyzrgb_.height; ++rr) {
      for (int cc = 0; cc < pcl_xyzrgb_.width; ++cc, ++pcl_iter) {
        input->access[rr][cc].r = pcl_iter->r;
        input->access[rr][cc].g = pcl_iter->g;
        input->access[rr][cc].b = pcl_iter->b;
        point_cloud->access[rr][cc].r = pcl_iter->x*100;
        point_cloud->access[rr][cc].g = pcl_iter->y*100;
        point_cloud->access[rr][cc].b = pcl_iter->z*100;
      }
  }
  //savePPM(point_cloud, "/home/sfoix/temp/point_cloud.ppm");

  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock(); 

  //std::cout << msg->data << std::endl; 

  // get parameters from dynamic reconfigure
  float sigma = alg_.get_sigma();
  float k = alg_.get_k();
  int min_size = alg_.get_min_size();
  // outputs
  int ** cluster_labels; 
  int num_ccs;
  int num_ccs_fin;

  image<rgb> *seg = segment_image(input, point_cloud, sigma, k, min_size, &num_ccs, &num_ccs_fin, cluster_labels);
  //savePPM(input, "/home/sfoix/temp/input_c.ppm");
  //savePPM(seg, "/home/sfoix/temp/segment_c.ppm");

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

  // Create publisher messages
  res.num_clusters.data = num_ccs_fin;
  res.cluster_labels.data.resize(pcl_xyzrgb_.width*pcl_xyzrgb_.width);
  int ii = 0;
  for (int rr=0; rr < pcl_xyzrgb_.height; ++rr)
    for (int cc=0; cc < pcl_xyzrgb_.width; ++cc, ++ii)
      res.cluster_labels.data[ii] = cluster_labels[rr][cc];

  // Create segmented rgb image for publishing it afterwards
  cv::Mat image(pcl_xyzrgb_.height, pcl_xyzrgb_.width, 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] = seg->access[rr][cc].r;    
      (*img_iter)[1] = seg->access[rr][cc].g;    
      (*img_iter)[2] = seg->access[rr][cc].b;    
    }                                                                      
  }                                                                          
//  imwrite("/home/sfoix/temp/input_cv.jpg", image);

  cv_ptr_ = boost::make_shared<cv_bridge::CvImage>();
  cv_ptr_->header = pcl_xyzrgb_.header;
  cv_ptr_->encoding = sensor_msgs::image_encodings::RGB8;
  cv_ptr_->image = image;

  // [publish messages]
  output_image_publisher_.publish(cv_ptr_->toImageMsg());

  return true;
}
/*  [action callbacks] */

/*  [action requests] */

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

  this->alg_.unlock();
}

void LeafSegmentationAlgNode::addNodeDiagnostics(void)
{
}

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