darwin_find_eggs_alg_node.cpp

Go to the documentation of this file.

#include "darwin_find_eggs_alg_node.h"
#include <pcl/filters/passthrough.h>
#include <tf/transform_broadcaster.h>

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

  // [init publishers]
  this->output_pcl2_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("output_pcl2", 1);
  
  // [init subscribers]
  this->input_pcl2_subscriber_ = this->public_node_handle_.subscribe("input_pcl2", 1, &DarwinFindEggsAlgNode::input_pcl2_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void DarwinFindEggsAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->PointCloud2_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->output_pcl2_publisher_.publish(this->PointCloud2_msg_);
}

/*  [subscriber callbacks] */
void DarwinFindEggsAlgNode::input_pcl2_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  //ROS_INFO("DarwinFindEggsAlgNode::input_pcl2_callback: New Message Received"); 

        pcl::PointCloud<pcl::PointXYZRGB> cloud;
        pcl::fromROSMsg (*msg, cloud);


        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_segmented (new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::PointXYZ *result=new pcl::PointXYZ[2];
  
  int pnumber = (int)cloud.size ();
/*  
  // Create the filtering object
  pcl::PassThrough<pcl::PointXYZRGB> pass;
  pass.setInputCloud (cloud.makeShared());
  pass.setFilterFieldName ("rgb");
  pass.setFilterLimits (200.0, 260.0);
  //pass.setFilterLimitsNegative (true);
  pass.filter (*cloud_segmented); 
  */
double mean_x=0.0, mean_y=0.0, mean_z=0.0;
int acum=0;
  pcl::copyPointCloud<pcl::PointXYZRGB>(cloud, *cloud_segmented);
  for (int point_id = 0; point_id < pnumber; ++point_id)
  {
    //intensity comes codified in the rgb field
    uint32_t rgb = *reinterpret_cast<int*>(&cloud.points[point_id].rgb);
    uint8_t r = (rgb >> 16) & 0x0000ff;
    //uint8_t g = (rgb >> 8) & 0x0000ff;
    //uint8_t b = (rgb) & 0x0000ff;
    
  //  ROS_INFO("DarwinFindEggsAlgNode:: RGB %d R%d  G%d B%d",rgb,r,g,b); 
    if ((r >200 ) && (cloud.points[point_id].z <0.5)) {
      //get the point
      mean_x+=cloud.points[point_id].x;
      mean_y+=cloud.points[point_id].y;
      mean_z+=cloud.points[point_id].z;
      acum++;
      //mark in red the point in the pointcloud
      // pack r/g/b into rgb
    uint8_t r = 255, g = 0, b = 0; // Red color
    uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
    cloud_segmented->points[point_id].rgb = *reinterpret_cast<float*>(&rgb);
    }
  }    
  //publish the transformation in the TF
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin (tf::Vector3 (mean_x/acum,mean_y/acum,mean_z/acum));
  transform.setRotation (tf::Quaternion (0,0,0,1));
  br.sendTransform (tf::StampedTransform (transform, ros::Time::now (), "/camboard_frame", "target_frame"));
  
  /*
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(centroid(0), centroid(1), centroid(2)) );
  transform.setRotation( tf::Quaternion(pose_c.x(), pose_c.y(), pose_c.z(), pose_c.w()) );
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/camboard_frame", "centroid"));
  */
  
/*
 *      // Segmentation
        pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
        // Create the segmentation object
        pcl::SACSegmentation<pcl::PointXYZ> seg;
        // Optional
        seg.setOptimizeCoefficients (true);
        // Mandatory
        seg.setModelType (pcl::SACMODEL_PLANE);
        seg.setMethodType (pcl::SAC_RANSAC);
        seg.setDistanceThreshold (0.02); // 1cm?

        // segment
        seg.setInputCloud (cloud.makeShared());
        seg.segment (*inliers, *coefficients);
        

    // Extract the planar inliers from the input cloud
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud (cloud.makeShared());
    extract.setIndices (inliers);

    // Remove the planar inliers, extract the rest
    extract.setNegative (true);
    extract.filter (*cloud_segmented); //*

        // copies all inliers of the model computed to another PointCloud
//      pcl::copyPointCloud<pcl::PointXYZ>(cloud, *inliers, *cloud_segmented);
*/


  if (output_pcl2_publisher_.getNumSubscribers () > 0) {
        pcl::toROSMsg(*cloud_segmented, this->PointCloud2_msg_);
        this->output_pcl2_publisher_.publish(this->PointCloud2_msg_);
  }





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

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

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

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

cv::Mat DarwinFindEggsAlgNode::pointcloud_to_image(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
        pcl::PointXYZRGB pixel;
        cv::Mat cvimage;
        uint32_t rgb;
        uint8_t r;
        uint8_t g;
        uint8_t b;

        
        cvimage = cv::Mat::zeros(cloud->height, cloud->width, CV_8UC3);//a black image 
        for (int i = 0; i < cvimage.rows; i++) {
                //double* Mi = cvimage.ptr<double>(i);
                
                for (int j = 0; j< cvimage.cols; j++) {
                        pixel = cloud->at(j,i);
                        
                        rgb = *reinterpret_cast<int*>(&pixel.rgb);
                        r = (rgb >> 16) & 0x0000ff;
                        g = (rgb >> 8)  & 0x0000ff;
                        b = (rgb)       & 0x0000ff;
                        //ROS_INFO_STREAM("Color: " << rgb );
                        if (pixel.x == pixel.x) { // if x != NaN
                                //Mi[j] = rgb;
                                cvimage.at<cv::Vec3b>(i,j)[0] = b;
                                cvimage.at<cv::Vec3b>(i,j)[1] = g;
                                cvimage.at<cv::Vec3b>(i,j)[2] = r;
                        }
                }
        }
        
        return cvimage;
}

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

  this->alg_.unlock();
}

void DarwinFindEggsAlgNode::addNodeDiagnostics(void)
{
}

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