cylinder_detector_alg_node.cpp

Go to the documentation of this file.

#include "cylinder_detector_alg_node.h"
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <tf/transform_datatypes.h>

using namespace std;

CylinderDetectorAlgNode::CylinderDetectorAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<CylinderDetectorAlgorithm>()
{
  ROS_INFO("Cylinder Detector Running");
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  marker_publisher_         = public_node_handle_.advertise<visualization_msgs::MarkerArray>("marker",         0);
  cloud_filtered_publisher_ = public_node_handle_.advertise<sensor_msgs::PointCloud2>       ("cloud_filtered", 0);
  pose_publisher_           = public_node_handle_.advertise<geometry_msgs::PoseStamped>     ("pose",           0);
  
  // [init subscribers]
  cloud_subscriber_ = public_node_handle_.subscribe("cloud", 0, &CylinderDetectorAlgNode::cloud_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
   
  
  MarkerArray_msg_.markers.resize(2);
}

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

void CylinderDetectorAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
}

/*  [subscriber callbacks] */
void CylinderDetectorAlgNode::cloud_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  ROS_DEBUG("CylinderDetectorAlgNode::cloud_callback: New Message Received"); 

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

  // Transformar pointcloud2 a pcl pointcloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
  pcl::fromROSMsg(*msg, *cloud);

  // Filtrar area
  float xmin=-3.0,xmax=3.0;
  float ymin=-3.0,ymax=3.0;
  float zmin=-1.0,zmax=1.0;
  ros::Time begin = ros::Time::now();
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud (cloud); 
  pass.setFilterFieldName ("x");
  pass.setFilterLimits (xmin, xmax);
  pass.filter (*cloud);
  pass.setFilterFieldName ("y");
  pass.setFilterLimits (ymin, ymax);
  pass.filter (*cloud);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (zmin, zmax);
  pass.filter (*cloud);
  ROS_DEBUG_STREAM("Filtered by fields: " << cloud->points.size ());
  ROS_DEBUG_STREAM("Fbf time:" << (ros::Time::now() - begin));

  // downsample
  begin = ros::Time::now();
  pcl::VoxelGrid<pcl::PointXYZ> vg;
  vg.setInputCloud (cloud);
  vg.setLeafSize (0.05f, 0.05f, 0.05f);
  vg.filter (*cloud);
  ROS_DEBUG_STREAM("Downsampled: " << cloud->points.size ());
  ROS_DEBUG_STREAM("VG time:" << (ros::Time::now() - begin));

  // remove outliers
  begin = ros::Time::now();
  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud (cloud);
  sor.setMeanK (50);
  sor.setStddevMulThresh (0.1);
  sor.filter (*cloud);
  ROS_DEBUG_STREAM("Removed outliers: " << cloud->points.size ());
  ROS_DEBUG_STREAM("SOR time:" << (ros::Time::now() - begin));

  begin = ros::Time::now();
  // trobar cilindre(s)
  //Estimate point normals
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  pcl::search::KdTree<pcl::PointXYZ>::Ptr  tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  pcl::PointCloud<pcl::Normal>::Ptr        cloud_normals (new pcl::PointCloud<pcl::Normal> ());
  ne.setSearchMethod (tree);
  ne.setInputCloud (cloud);
  ne.setKSearch (5);
  ne.compute (*cloud_normals);
  ROS_DEBUG_STREAM("Normals time:" << (ros::Time::now() - begin));

  // Create the segmentation object for cylinder segmentation and set all the parameters
  begin = ros::Time::now();
  pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
  pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr      inliers_cylinder (new pcl::PointIndices);
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_CYLINDER);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setNormalDistanceWeight (0.1);
  seg.setMaxIterations (5000);
  seg.setDistanceThreshold (0.02);
  seg.setRadiusLimits (0.15,0.25);
  seg.setInputCloud (cloud);
  seg.setInputNormals (cloud_normals);
  seg.segment (*inliers_cylinder, *coefficients_cylinder);
  ROS_DEBUG_STREAM("Cylinder coefficients: " << *coefficients_cylinder);
  ROS_DEBUG_STREAM("Seg time:" << (ros::Time::now() - begin));

  begin = ros::Time::now();
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  extract.setInputCloud (cloud);
  extract.setIndices (inliers_cylinder);
  extract.setNegative (false);
  extract.filter (*cloud);
  ROS_DEBUG_STREAM("Extracted: " << cloud->points.size ());
  ROS_DEBUG_STREAM("Extract time:" << (ros::Time::now() - begin));
  
  // Cylinder Cloud Publisher
  pcl::toROSMsg(*cloud,PointCloud2_msg_);
  cloud_filtered_publisher_.publish(PointCloud2_msg_);

  if( cloud->points.size()>0)
  {
    // POSE
    PoseStamped_msg_.header.stamp    = ros::Time::now();
    PoseStamped_msg_.header.frame_id = msg->header.frame_id;
    PoseStamped_msg_.pose.position.x = coefficients_cylinder->values[0];
    PoseStamped_msg_.pose.position.y = coefficients_cylinder->values[1];
    PoseStamped_msg_.pose.position.z = coefficients_cylinder->values[2];
    PoseStamped_msg_.pose.orientation.x = PoseStamped_msg_.pose.orientation.y = PoseStamped_msg_.pose.orientation.z = 0;
    PoseStamped_msg_.pose.orientation.w = 1;
    pose_publisher_.publish(PoseStamped_msg_);
    // MARKERS ADD
    MarkerArray_msg_.markers[0].action = MarkerArray_msg_.markers[1].action = visualization_msgs::Marker::ADD;
     // CYLINDER AND TEXT
    MarkerArray_msg_.markers[0].header.frame_id = MarkerArray_msg_.markers[1].header.frame_id = msg->header.frame_id;
    MarkerArray_msg_.markers[0].header.stamp = MarkerArray_msg_.markers[1].header.stamp = ros::Time::now();
    MarkerArray_msg_.markers[0].id = MarkerArray_msg_.markers[1].id = 0;
    MarkerArray_msg_.markers[0].pose = MarkerArray_msg_.markers[1].pose = PoseStamped_msg_.pose;
    MarkerArray_msg_.markers[0].lifetime = MarkerArray_msg_.markers[1].lifetime = ros::Duration();
    //red
    MarkerArray_msg_.markers[0].color.r = MarkerArray_msg_.markers[1].color.r = 1.0;
    MarkerArray_msg_.markers[0].color.g = MarkerArray_msg_.markers[1].color.g = 0.0;
    MarkerArray_msg_.markers[0].color.b = MarkerArray_msg_.markers[1].color.b = 0.0;
    // ONLY CYLINDER
    MarkerArray_msg_.markers[0].ns = "cylinder";
    MarkerArray_msg_.markers[0].type = visualization_msgs::Marker::CYLINDER;
    MarkerArray_msg_.markers[0].pose.position.z = -0.2;
    MarkerArray_msg_.markers[0].scale.x = 0.4;
    MarkerArray_msg_.markers[0].scale.y = 0.4;
    MarkerArray_msg_.markers[0].scale.z = 2;
    MarkerArray_msg_.markers[0].color.a = 0.4;
    // ONLY TEXT
    MarkerArray_msg_.markers[1].ns = "text";
    MarkerArray_msg_.markers[1].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    MarkerArray_msg_.markers[1].text = alg_.config_.markertext;
    MarkerArray_msg_.markers[1].pose.position.z = 1;
    MarkerArray_msg_.markers[1].scale.z = 0.3;
    MarkerArray_msg_.markers[1].color.a = 1.0;
  }else{
    // MARKERS DELETE
    MarkerArray_msg_.markers[0].action = MarkerArray_msg_.markers[1].action = visualization_msgs::Marker::DELETE; 
  }
  
  marker_publisher_.publish(MarkerArray_msg_);  
    
  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->cloud_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

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

  this->alg_.unlock();
}

void CylinderDetectorAlgNode::addNodeDiagnostics(void)
{
}

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