iri_clothes_grasping_point_alg_node.cpp

Go to the documentation of this file.

#include "iri_clothes_grasping_point_alg_node.h"

using namespace cv;
namespace enc = sensor_msgs::image_encodings;

static const char WINDOW[] = "Image window";
static const char WINDOW2[] = "Auxiliar window";

IriClothesGraspingPointAlgNode::IriClothesGraspingPointAlgNode(void) : imgtransport_(public_node_handle_) 
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  this->image_pub_ = imgtransport_.advertise("image/out", 1);
  this->image_sub_ = imgtransport_.subscribe("image/in", 1, &IriClothesGraspingPointAlgNode::image_callback, this);
  this->image_pub1_ = imgtransport_.advertise("image/out/1", 1);
  this->image_pub2_ = imgtransport_.advertise("image/out/2", 1);
  this->image_pub3_ = imgtransport_.advertise("image/out/3", 1);
  this->image_pub4_ = imgtransport_.advertise("image/out/4", 1);
  this->image_pub5_ = imgtransport_.advertise("image/out/5", 1);
  this->image_pub6_ = imgtransport_.advertise("image/out/6", 1);
  this->image_pub_result_ = imgtransport_.advertise("image/out/result", 1);

  cv::namedWindow(WINDOW);
  cv::namedWindow(WINDOW2);

  // [init publishers]
  this->point_list_publisher_ = this->public_node_handle_.advertise<iri_clothes_grasping_point::GraspingPointList>("point_list", 5);
  this->poses_list_publisher_ = this->public_node_handle_.advertise<iri_clothes_grasping_point::GraspingPointList3d>("point3d_list", 5);
  
  // [init subscribers]
  this->pcl2_sub_ = this->public_node_handle_.subscribe("input/pcl2/segmented", 1, &IriClothesGraspingPointAlgNode::pcl2_segmented_callback, this); 
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

void IriClothesGraspingPointAlgNode::binaryObjectSelection(const cv::Mat labelled_image, cv::Mat& binary_image, unsigned char Red, unsigned char Green, unsigned char Blue)
{

  cvtColor( labelled_image, binary_image, CV_BGR2GRAY );

  if(labelled_image.channels() == 3){
//    ROS_INFO("Image depth is 3 assuming BGR");
    for(int i=0;i<binary_image.size().height;i++){
      for(int j=0;j<binary_image.size().width;j++){
        if(labelled_image.at<Vec3b>(i,j)[0] == Blue && labelled_image.at<Vec3b>(i,j)[1] == Green && labelled_image.at<Vec3b>(i,j)[2] == Red){
          binary_image.at<uchar>(i,j) = 255;
        }else{
//          ROS_INFO(" %d %d %d ", labelled_image.at<Vec3b>(i,j)[0],labelled_image.at<Vec3b>(i,j)[1],labelled_image.at<Vec3b>(i,j)[2]);   
          binary_image.at<uchar>(i,j) = 0;
        }
      } 
    } 
  }else{
    ROS_ERROR("Incorrect number of channels"); 
  }
  
}

void IriClothesGraspingPointAlgNode::draw_contours_center(cv::Mat binary_img, cv::Mat& color_output)
{
  RNG rng(12345);

  cv::Mat canny_output;
  vector<vector<cv::Point> > contours;
  vector<cv::Vec4i> hierarchy;

  Canny( binary_img, canny_output, canny_threshold_, canny_threshold_*2, 3 );
  findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );

  vector<Moments> mu(contours.size() );
  for( int i = 0; i < contours.size(); i++ ){
    mu[i] = moments( contours[i], false );
  }

  vector<Point2f> mc( contours.size() );
  for( int i = 0; i < contours.size(); i++ ){
    mc[i] = Point2f( mu[i].m10/mu[i].m00 , mu[i].m01/mu[i].m00 );
  }

  color_output = Mat::zeros( canny_output.size(), CV_8UC3 );
  for( int i = 0; i < contours.size(); i++ ) {
    if(mu[i].m00 > minarea_){
      ROS_INFO(" * Contour[%d] - Area (M_00) = %.2f (> %.2f) - Area OpenCV: %.2f - Length: %.2f Grasp point: %.2f %.2f", i, mu[i].m00, minarea_, contourArea(contours[i]), arcLength( contours[i], true ), mc[i].x, mc[i].y );
       Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
       drawContours( color_output, contours, i, color, 2, 8, hierarchy, 0, Point() );
       circle( color_output, mc[i], 4, color, -1, 8, 0 );

        pixel_msg_.xpixel = mc[i].x; 
        pixel_msg_.ypixel = mc[i].y; 
        pixel_msg_.area = mu[i].m00; 
        pixel_msg_.length = arcLength( contours[i], true); 

        //grasping_point_list_.push_back(mc[i]);
        graspingPointList_msg_.pixels.push_back(pixel_msg_); 
      
     }
  }

//  printf("\t Info: Area and Contour Length \n");
//  for( int i = 0; i < contours.size(); i++ ) {
//    if(mu[i].m00 > minarea_){
//      ROS_INFO(" * Contour[%d] - Area (M_00) = %.2f - Area OpenCV: %.2f - Length: %.2f \n", i, mu[i].m00, contourArea(contours[i]), arcLength( contours[i], true ) );
//      Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
//      drawContours( drawing, contours, i, color, 2, 8, hierarchy, 0, Point() );
//      circle( drawing, mc[i], 4, color, -1, 8, 0 );
//    }
//  }
}

void IriClothesGraspingPointAlgNode::count_labels(cv::Mat labelled_image, std::set<unsigned int>& labels){

//TODO getparam to prevent nodes counting over and over again?

  int r,g,b;
  unsigned int rgb;
  std::set<unsigned int>::iterator it;
    
  //check channels
  if(labelled_image.channels() != 3){
    ROS_ERROR("Incorrect image depth");
    return;
  }

  labels.clear();
  for(int i=0;i<labelled_image.size().height;i++){
    for(int j=0;j<labelled_image.size().width;j++){
      b = labelled_image.at<Vec3b>(i,j)[0];
      g = labelled_image.at<Vec3b>(i,j)[1];
      r = labelled_image.at<Vec3b>(i,j)[2];
      rgb = rgb2double(r,g,b);
      labels.insert(rgb);
    }
  }
  ROS_INFO("%d labels found", labels.size());

  //ROS_INFO("my set contains: ");
  for ( it=labels.begin(); it != labels.end(); it++ ){
    rgb = *it;
    double2rgb(r,g,b,rgb);
    //ROS_INFO(" %d %d %d %d ",r,g,b,rgb);
  }
}

void IriClothesGraspingPointAlgNode::draw_clothes_centers(cv::Mat& src_img){

  cv::Mat str_element = Mat();
  cvtColor(src_img, global_cloth_center_, CV_BGR2GRAY );

  global_cloth_center_.colRange(0,2).setTo(0);
  global_cloth_center_.rowRange(0,2).setTo(0);
  global_cloth_center_.colRange(global_cloth_center_.size().width-2,global_cloth_center_.size().width).setTo(0);
  global_cloth_center_.rowRange(global_cloth_center_.size().height-2,global_cloth_center_.size().height).setTo(0);

//      cv_image.header.stamp = ros::Time::now();
//      cv_image.header.frame_id = "camera";
//      cv_image.encoding = "mono8"; 
//      cv_image.image = global_cloth_center_;
//  image_pub5_.publish(cv_image.toImageMsg());

  draw_contours_center(global_cloth_center_, global_cloth_center_color_);

}

void IriClothesGraspingPointAlgNode::draw_intersections(cv::Mat& src_img){


  cv::Mat str_element = Mat();
  std::set<unsigned int> labels;
  std::set<unsigned int>::iterator it1;
  std::set<unsigned int>::iterator it2;
  int numlabels;
  int r1,g1,b1;
  int r2,g2,b2;
  unsigned int rgb1;
  unsigned int rgb2;

  count_labels(src_img, labels);
  numlabels = labels.size();
  
  for ( it1=labels.begin(); it1 != labels.end(); it1++ ){
    for ( it2 = it1; it2 != labels.end(); it2++ ){
      rgb1 = *it1;
      rgb2 = *it2;
      double2rgb(r1,g1,b1,rgb1);
      double2rgb(r2,g2,b2,rgb2);
      if(r1 == 0 && g1 == 0 && b1 == 0 
      || r2 == 0 && g2 == 0 && b2 == 0
      || rgb1 == rgb2) 
        continue; 

//      ROS_INFO("%d %d %d %d AND %d %d %d %d ",r1,g1,b1,rgb1,r2,g2,b2,rgb2);

//      r1 = 1;
//      g1 = 0;
//      b1 = 128;
//      r2 = 128;
//      g2 = 0;
//      b2 = 0;

//      r1 = 128;
//      g1 = 127;
//      b1 = 254;
//      r2 = 128;
//      g2 = 0;
//      b2 = 0;

      //select first label
      binaryObjectSelection(src_img, binary_img1_, r1, g1, b1);

      cv_image.header.stamp = ros::Time::now();
      cv_image.header.frame_id = "camera";
      cv_image.encoding = "mono8"; 
      cv_image.image = binary_img1_;
      image_pub1_.publish(cv_image.toImageMsg());

      //select second label
      binaryObjectSelection(src_img, binary_img2_, r2, g2, b2);

      cv_image.image = binary_img2_;
      cv_image.header.stamp = ros::Time::now();
      image_pub2_.publish(cv_image.toImageMsg());

      //dilate
      dilate(binary_img1_, binary_img1_, str_element);
      dilate(binary_img2_, binary_img2_, str_element);

      //intersect
      bitwise_and(binary_img1_, binary_img2_, intersect_);

      cv_image.image = intersect_;
      cv_image.header.stamp = ros::Time::now();
      image_pub3_.publish(cv_image.toImageMsg());

      //sum
      bitwise_or(intersect_, global_intersect_, global_intersect_);

      cv_image.header.stamp = ros::Time::now();
      cv_image.image = global_intersect_;
      image_pub4_.publish(cv_image.toImageMsg());
      sleep(1);
    }
  }

   //compute intersections moments to obtain grasping center
  draw_contours_center(global_intersect_, src_img ); 
}

void IriClothesGraspingPointAlgNode::image_processing(const cv::Mat src_img){

  //check image size changes
  if(global_cloth_center_.empty() || src_img.size().height != global_cloth_center_.size().height
  || src_img.size().width != global_cloth_center_.size().width){
    binary_img1_ = cv::Mat(src_img.size(),CV_8UC1);
    binary_img2_ = cv::Mat(src_img.size(),CV_8UC1);
    intersect_ = cv::Mat(src_img.size(),CV_8UC1);
    global_intersect_ = cv::Mat(src_img.size(),CV_8UC1);
    global_intersect_color_ = cv::Mat(src_img.size(),CV_8UC1);
    global_cloth_center_ = cv::Mat(src_img.size(),CV_8UC1);
    global_cloth_center_color_ = cv::Mat(src_img.size(),CV_8UC3);
  }

  graspingPointList_msg_.pixels.clear();

  if(!intersections_){

      ROS_INFO("Compute and draw moments");
      // Moments drawing
      global_cloth_center_color_ = src_img.clone();
      draw_clothes_centers(global_cloth_center_color_);
      //cv::imshow(WINDOW, global_cloth_center_color_ );

      cv_image.image = global_cloth_center_color_; 

  }else{

      ROS_INFO("Intersection heat map");
      // Intersection heat_map
      global_intersect_color_ = src_img.clone();
      draw_intersections(global_intersect_color_);

      cv_image.image = global_intersect_color_; 

      ROS_INFO("Done intersection heat map");
      //cv::imshow(WINDOW2, global_intersect_color_);
  }
  cv_image.header.stamp = ros::Time::now();
  cv_image.header.frame_id = "camera";
  cv_image.encoding = "bgr8"; 
  image_pub_result_.publish(cv_image.toImageMsg());

  //cv::waitKey(1);

}

void IriClothesGraspingPointAlgNode::image_callback(const sensor_msgs::ImageConstPtr& msg) {
  cv::Mat src_gray;
  cv_bridge::CvImagePtr cv_ptr;

  try
  {
    cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

  image_processing(cv_ptr->image);

  image_pub_.publish(cv_ptr->toImageMsg());

  graspingPointList_msg_.header = msg->header;
  point_list_publisher_.publish(graspingPointList_msg_); 

}

void IriClothesGraspingPointAlgNode::image_from_pcl2(cv::Mat& rgb_image){

  pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter = this->pcl_xyzrgb_.begin();

  rgb_image.create(this->pcl_xyzrgb_.height, this->pcl_xyzrgb_.width, CV_8UC3);
  for (int rr = 0; rr < (int)this->pcl_xyzrgb_.height; ++rr) {
    for (int cc = 0; cc < (int)this->pcl_xyzrgb_.width; ++cc, ++pt_iter) {
      pcl::PointXYZRGB &pt = *pt_iter;
      RGBValue color;
      color.float_value = pt.rgb;
      rgb_image.at<Vec3b>(rr,cc)[0] = color.Blue;
      rgb_image.at<Vec3b>(rr,cc)[1] = color.Green;
      rgb_image.at<Vec3b>(rr,cc)[2] = color.Red;
    }
  }
}

void IriClothesGraspingPointAlgNode::pcl2_segmented_callback(const sensor_msgs::PointCloud2ConstPtr& msg)
{

  cv::Mat rgb_image;
  pcl::PointXYZRGB point;
  ROS_INFO("Received segmented point cloud");
  pcl::fromROSMsg(*msg, this->pcl_xyzrgb_);
  image_from_pcl2(rgb_image);

  cv_image.header.stamp = ros::Time::now();
  cv_image.header.frame_id = "camera";
  cv_image.encoding = "bgr8"; 
  cv_image.image = rgb_image;
  image_pub_.publish(cv_image.toImageMsg());

  image_processing(rgb_image); //fill graspingPointList

  graspingPointList3d_msg_.poses.clear();
  graspingPointList3d_msg_.poses.resize(graspingPointList_msg_.pixels.size());
  for(int i=0; i<graspingPointList_msg_.pixels.size();i++){

    point = pcl_xyzrgb_(graspingPointList_msg_.pixels[i].xpixel,graspingPointList_msg_.pixels[i].ypixel);
    graspingPointList3d_msg_.poses[i].position.x = point.x;
    graspingPointList3d_msg_.poses[i].position.y = point.y;
    graspingPointList3d_msg_.poses[i].position.z = point.z;
    graspingPointList3d_msg_.poses[i].orientation.x = 0;
    graspingPointList3d_msg_.poses[i].orientation.y = 0;
    graspingPointList3d_msg_.poses[i].orientation.z = 0;
    graspingPointList3d_msg_.poses[i].orientation.w = sqrt(2)/2;

  }

  ROS_INFO("Publishing points and image"); 


  graspingPointList3d_msg_.header = msg->header;
  poses_list_publisher_.publish(graspingPointList3d_msg_); 

  graspingPointList_msg_.header = msg->header;
  point_list_publisher_.publish(graspingPointList_msg_); 

}

IriClothesGraspingPointAlgNode::~IriClothesGraspingPointAlgNode(void)
{
  // [free dynamic memory]
    cv::destroyWindow(WINDOW);
    cv::destroyWindow(WINDOW2);
}

void IriClothesGraspingPointAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->GraspingPointList_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]
//  this->point_list_publisher_.publish(this->GraspingPointList_msg_);
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void IriClothesGraspingPointAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
  ROS_INFO("Reconfigure request : %d %d %d %d", (int)config.minarea, config.cannyth, config.cannymaxth, (int)config.intersections);
  
    minarea_ = config.minarea;
    canny_threshold_ = config.cannyth;
    canny_max_thresh_ = config.cannymaxth;
    intersections_ = config.intersections;

  this->alg_.unlock();
}

void IriClothesGraspingPointAlgNode::addNodeDiagnostics(void)
{
}

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