Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00046
00047 #include <ros/ros.h>
00048
00049 #include "pcl/io/pcd_io.h"
00050 #include "pcl/point_types.h"
00051 #include <pcl/segmentation/segment_differences.h>
00052
00053 #include "pcl_ros/publisher.h"
00054 #include "pcl_ros/subscriber.h"
00055
00056
00057 #include <pcl/filters/radius_outlier_removal.h>
00058 using namespace std;
00059
00060 class SegmentDifferencesNode
00061 {
00062 protected:
00063 ros::NodeHandle nh_;
00064
00065 public:
00066 string output_cloud_topic_, input_cloud_topic_;
00067 string output_filtered_cloud_topic_;
00068
00069 pcl_ros::Publisher<pcl::PointXYZ> pub_diff_;
00070 pcl_ros::Publisher<pcl::PointXYZ> pub_filtered_;
00071 pcl_ros::Subscriber<sensor_msgs::PointCloud2> sub_;
00072
00073 pcl::SegmentDifferences <pcl::PointXYZ> seg_;
00074 pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem_;
00075 double rate_;
00076 int counter_;
00077 double distance_threshold_;
00079 SegmentDifferencesNode (ros::NodeHandle &n) : nh_(n)
00080 {
00081
00082 nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/cloud_pcd"));
00083 nh_.param("output_cloud_topic", output_cloud_topic_, std::string("difference"));
00084 nh_.param("output_filtered_cloud_topic", output_filtered_cloud_topic_, std::string("difference_filtered"));
00085 nh_.param("distance_threshold", distance_threshold_, 0.0005);
00086 ROS_INFO ("Distance threshold set to %lf.", distance_threshold_);
00087 pub_diff_.advertise (nh_, output_cloud_topic_.c_str (), 1);
00088 ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ());
00089 pub_filtered_.advertise (nh_, output_filtered_cloud_topic_.c_str (), 1);
00090 ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_filtered_cloud_topic_).c_str ());
00091
00092 sub_.subscribe (nh_, input_cloud_topic_, 1, boost::bind (&SegmentDifferencesNode::cloud_cb, this, _1));
00093 ROS_INFO ("Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
00094
00095 seg_.setDistanceThreshold (distance_threshold_);
00096
00097 rate_ = 1;
00098 counter_ = 0;
00099 }
00100
00102
00103 void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00104 {
00105 pcl::PointCloud<pcl::PointXYZ> cloud_in;
00106 pcl::PointCloud<pcl::PointXYZ> output;
00107 pcl::PointCloud<pcl::PointXYZ> output_filtered;
00108 pcl::fromROSMsg(*pc, cloud_in);
00109
00110 if (counter_ == 0)
00111 {
00112 ROS_INFO("Setting input cloud with %ld points", cloud_in.points.size());
00113 seg_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00114 counter_++;
00115 }
00116 else
00117 {
00118 ROS_INFO("Setting target cloud with %ld points", cloud_in.points.size());
00119 seg_.setTargetCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00120 seg_.segment (output);
00121 counter_ = 0;
00122 ROS_INFO("Publishing difference cloud with %ld points", output.points.size());
00123 pub_diff_.publish (output);
00124 outrem_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(output));
00125 outrem_.setRadiusSearch (0.02);
00126 outrem_.setMinNeighborsInRadius (15);
00127 outrem_.filter (output_filtered);
00128 pub_filtered_.publish (output_filtered);
00129 }
00130 }
00131 };
00132
00133 int main (int argc, char** argv)
00134 {
00135 ros::init (argc, argv, "segment_difference_node");
00136 ros::NodeHandle n("~");
00137 SegmentDifferencesNode sd(n);
00138 ros::spin ();
00139 return (0);
00140 }
00141
00142