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/filters/radius_outlier_removal.h>
00055 #include "pcl/segmentation/extract_clusters.h"
00056 #include <tf/transform_listener.h>
00057 #include "pcl/common/common.h"
00058 #include "pcl_ros/transforms.h"
00059
00060 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00061 typedef pcl::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
00062
00063 using namespace std;
00064
00065 class SegmentDifferencesNode
00066 {
00067 protected:
00068 ros::NodeHandle nh_;
00069
00070 public:
00071 string output_cloud_topic_, input_cloud_topic_;
00072 string output_filtered_cloud_topic_;
00073
00074 pcl_ros::Publisher<pcl::PointXYZ> pub_diff_;
00075 pcl_ros::Publisher<pcl::PointXYZ> pub_filtered_;
00076 ros::Subscriber sub_;
00077
00078 pcl::SegmentDifferences <pcl::PointXYZ> seg_;
00079 pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem_;
00080 pcl::EuclideanClusterExtraction<pcl::PointXYZ> cluster_;
00081 KdTreePtr clusters_tree_;
00082 double rate_;
00083 int counter_;
00084 double distance_threshold_;
00085 bool segment_, take_first_cloud_, save_segmented_cloud_;
00086 double object_cluster_tolerance_;
00087 int object_cluster_min_size_, object_cluster_max_size_;
00088 tf::TransformListener listener_;
00090 SegmentDifferencesNode (ros::NodeHandle &n) : nh_(n)
00091 {
00092
00093 nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/cloud_pcd"));
00094 nh_.param("output_cloud_topic", output_cloud_topic_, std::string("difference"));
00095 nh_.param("output_filtered_cloud_topic", output_filtered_cloud_topic_, std::string("difference_filtered"));
00096 nh_.param("distance_threshold", distance_threshold_, 0.01);
00097 nh_.param("save_segmented_cloud_", save_segmented_cloud_, true);
00098 ROS_INFO ("Distance threshold set to %lf.", distance_threshold_);
00099 pub_diff_.advertise (nh_, output_cloud_topic_.c_str (), 1);
00100 ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ());
00101 pub_filtered_.advertise (nh_, output_filtered_cloud_topic_.c_str (), 1);
00102 ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_filtered_cloud_topic_).c_str ());
00103 sub_ = nh_.subscribe (input_cloud_topic_, 1, &SegmentDifferencesNode::cloud_cb, this);
00104 ROS_INFO ("Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
00105 nh_.param("object_cluster_tolerance", object_cluster_tolerance_, 0.1);
00106
00107 nh_.param("object_cluster_min_size", object_cluster_min_size_, 500);
00108
00109 seg_.setDistanceThreshold (distance_threshold_);
00110 clusters_tree_ = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
00111 clusters_tree_->setEpsilon (1);
00112 rate_ = 1;
00113 counter_ = 0;
00114 segment_ = take_first_cloud_ = false;
00115 }
00116
00118
00119 void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00120 {
00121 pcl::PointCloud<pcl::PointXYZ> cloud_in;
00122 pcl::PointCloud<pcl::PointXYZ> output;
00123 pcl::PointCloud<pcl::PointXYZ> output_filtered;
00124 pcl::fromROSMsg(*pc, cloud_in);
00125 nh_.getParam("/segment_difference_interactive/segment", segment_);
00126 nh_.getParam("/segment_difference_interactive/take_first_cloud", take_first_cloud_);
00127
00128 if (take_first_cloud_)
00129 {
00130 ROS_INFO("Setting target cloud with %ld points", cloud_in.points.size());
00131
00132 seg_.setTargetCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00133 counter_++;
00134 nh_.setParam("/segment_difference_interactive/take_first_cloud", false);
00135 pub_diff_.publish (cloud_in);
00136 }
00137 else if (segment_)
00138 {
00139 ROS_INFO("Setting input cloud with %ld points", cloud_in.points.size());
00140
00141 seg_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud_in));
00142 seg_.segment (output);
00143
00144 outrem_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(output));
00145 outrem_.setRadiusSearch (0.02);
00146 outrem_.setMinNeighborsInRadius (10);
00147 outrem_.filter (output_filtered);
00148
00149
00150 std::vector<pcl::PointIndices> clusters;
00151 cluster_.setInputCloud (boost::make_shared<PointCloud>(output_filtered));
00152 cluster_.setClusterTolerance (object_cluster_tolerance_);
00153 cluster_.setMinClusterSize (object_cluster_min_size_);
00154
00155 cluster_.setSearchMethod (clusters_tree_);
00156 cluster_.extract (clusters);
00157 ROS_INFO("[SegmentDifferencesNode:] Found %ld cluster", clusters.size());
00158
00159
00160 tf::StampedTransform transform;
00161 listener_.waitForTransform("map", "base_laser_link", ros::Time(), ros::Duration(10.0));
00162 try
00163 {
00164 listener_.lookupTransform("map", "base_laser_link", ros::Time(), transform);
00165 }
00166 catch (tf::TransformException ex)
00167 {
00168 ROS_ERROR("getTransformIn %s",ex.what());
00169 }
00170
00171
00172 pcl::PointCloud<pcl::PointXYZ> cloud_object_clustered;
00173 btVector3 robot_position = transform.getOrigin();
00174 btVector3 cluster_center;
00175 pcl::PointXYZ point_min;
00176 pcl::PointXYZ point_max;
00177 btScalar dist = 100.00;
00178 int closest_cluster = 0;
00179 if (clusters.size() > 0)
00180 {
00181 for (unsigned int i = 0; i < clusters.size(); i++)
00182 {
00183 pcl::copyPointCloud (output_filtered, clusters[i], cloud_object_clustered);
00184 getMinMax3D (cloud_object_clustered, point_min, point_max);
00185
00186 cluster_center.setX((point_max.x + point_min.x)/2);
00187 cluster_center.setY((point_max.y + point_min.y)/2);
00188 cluster_center.setZ((point_max.z + point_min.z)/2);
00189 if (dist > fabs(robot_position.distance(cluster_center)))
00190 {
00191 closest_cluster = i;
00192 dist = fabs(robot_position.distance(cluster_center));
00193 }
00194 ROS_INFO("Robot's distance to cluster %d is %fm", i, fabs(robot_position.distance(cluster_center)));
00195 cloud_object_clustered.points.clear();
00196 }
00197 ROS_INFO("Closest cluster: %d", closest_cluster);
00198 pcl::copyPointCloud (output_filtered, clusters[closest_cluster], cloud_object_clustered);
00199 ROS_INFO("Publishing difference cloud with %ld points to topic %s", cloud_object_clustered.points.size(), output_filtered_cloud_topic_.c_str());
00200 pub_filtered_.publish (cloud_object_clustered);
00201 if (save_segmented_cloud_)
00202 {
00203 pcl::PCDWriter writer;
00204 std::stringstream furniture_face;
00205 furniture_face << "furniture_face_" << ros::Time::now() << ".pcd";
00206 ROS_INFO("Writing to file: %s", furniture_face.str().c_str());
00207
00208 bool found_transform = listener_.waitForTransform(cloud_object_clustered.header.frame_id, "map",
00209 cloud_object_clustered.header.stamp, ros::Duration(10.0));
00210 if (found_transform)
00211 {
00212 pcl::PointCloud<pcl::PointXYZ> output_cloud;
00213
00214 tf::StampedTransform transform;
00215 listener_.lookupTransform("map", cloud_object_clustered.header.frame_id, cloud_object_clustered.header.stamp, transform);
00216 Eigen::Matrix4f transform_matrix;
00217 pcl_ros::transformAsMatrix (transform, transform_matrix);
00218 pcl::transformPointCloud(cloud_object_clustered, output_cloud, transform_matrix);
00219 writer.write (furniture_face.str(), output_cloud, false);
00220 }
00221 }
00222 }
00223
00224
00225 segment_ = false;
00226 nh_.setParam("/segment_difference_interactive/segment", false);
00227 }
00228 else
00229 return;
00230 }
00231 };
00232
00233 int main (int argc, char** argv)
00234 {
00235 ros::init (argc, argv, "segment_difference_node");
00236 ros::NodeHandle n("~");
00237 SegmentDifferencesNode sd(n);
00238 ros::spin ();
00239 return (0);
00240 }
00241
00242