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
00038 #include <pluginlib/class_list_macros.h>
00039 #include <pcl/io/io.h>
00040 #include <pcl/PointIndices.h>
00041 #include "pcl_ros/segmentation/extract_clusters.h"
00042
00043 #include <pcl_conversions/pcl_conversions.h>
00044
00045 using pcl_conversions::fromPCL;
00046 using pcl_conversions::moveFromPCL;
00047 using pcl_conversions::toPCL;
00048
00050 void
00051 pcl_ros::EuclideanClusterExtraction::onInit ()
00052 {
00053
00054 PCLNodelet::onInit ();
00055
00056
00057 double cluster_tolerance;
00058 if (!pnh_->getParam ("cluster_tolerance", cluster_tolerance))
00059 {
00060 NODELET_ERROR ("[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", getName ().c_str ());
00061 return;
00062 }
00063 int spatial_locator;
00064 if (!pnh_->getParam ("spatial_locator", spatial_locator))
00065 {
00066 NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
00067 return;
00068 }
00069
00070
00071 pnh_->getParam ("publish_indices", publish_indices_);
00072
00073 if (publish_indices_)
00074 pub_output_ = pnh_->advertise<PointIndices> ("output", max_queue_size_);
00075 else
00076 pub_output_ = pnh_->advertise<PointCloud> ("output", max_queue_size_);
00077
00078
00079 srv_ = boost::make_shared <dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > (*pnh_);
00080 dynamic_reconfigure::Server<EuclideanClusterExtractionConfig>::CallbackType f = boost::bind (&EuclideanClusterExtraction::config_callback, this, _1, _2);
00081 srv_->setCallback (f);
00082
00083
00084 if (use_indices_)
00085 {
00086
00087 sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00088 sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00089
00090 if (approximate_sync_)
00091 {
00092 sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud, PointIndices> > > (max_queue_size_);
00093 sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
00094 sync_input_indices_a_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
00095 }
00096 else
00097 {
00098 sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud, PointIndices> > > (max_queue_size_);
00099 sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
00100 sync_input_indices_e_->registerCallback (bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, _2));
00101 }
00102 }
00103 else
00104
00105 sub_input_ = pnh_->subscribe<PointCloud> ("input", max_queue_size_, bind (&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr ()));
00106
00107 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00108 " - max_queue_size : %d\n"
00109 " - use_indices : %s\n"
00110 " - cluster_tolerance : %f\n",
00111 getName ().c_str (),
00112 max_queue_size_,
00113 (use_indices_) ? "true" : "false", cluster_tolerance);
00114
00115
00116 impl_.setClusterTolerance (cluster_tolerance);
00117 }
00118
00120 void
00121 pcl_ros::EuclideanClusterExtraction::config_callback (EuclideanClusterExtractionConfig &config, uint32_t level)
00122 {
00123 if (impl_.getClusterTolerance () != config.cluster_tolerance)
00124 {
00125 impl_.setClusterTolerance (config.cluster_tolerance);
00126 NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance);
00127 }
00128 if (impl_.getMinClusterSize () != config.cluster_min_size)
00129 {
00130 impl_.setMinClusterSize (config.cluster_min_size);
00131 NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size);
00132 }
00133 if (impl_.getMaxClusterSize () != config.cluster_max_size)
00134 {
00135 impl_.setMaxClusterSize (config.cluster_max_size);
00136 NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size);
00137 }
00138 if (max_clusters_ != config.max_clusters)
00139 {
00140 max_clusters_ = config.max_clusters;
00141 NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters);
00142 }
00143 }
00144
00146 void
00147 pcl_ros::EuclideanClusterExtraction::input_indices_callback (
00148 const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
00149 {
00150
00151 if (pub_output_.getNumSubscribers () <= 0)
00152 return;
00153
00154
00155 if (!isValid (cloud))
00156 {
00157 NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
00158 return;
00159 }
00160
00161 if (indices && !isValid (indices))
00162 {
00163 NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
00164 return;
00165 }
00166
00168 if (indices) {
00169 std_msgs::Header cloud_header = fromPCL(cloud->header);
00170 std_msgs::Header indices_header = indices->header;
00171 NODELET_DEBUG ("[%s::input_indices_callback]\n"
00172 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00173 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00174 getName ().c_str (),
00175 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud_header.stamp.toSec (), cloud_header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00176 indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00177 } else {
00178 NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
00179 }
00181
00182 IndicesPtr indices_ptr;
00183 if (indices)
00184 indices_ptr.reset (new std::vector<int> (indices->indices));
00185
00186 impl_.setInputCloud (cloud);
00187 impl_.setIndices (indices_ptr);
00188
00189 std::vector<pcl::PointIndices> clusters;
00190 impl_.extract (clusters);
00191
00192 if (publish_indices_)
00193 {
00194 for (size_t i = 0; i < clusters.size (); ++i)
00195 {
00196 if ((int)i >= max_clusters_)
00197 break;
00198
00199 pcl_msgs::PointIndices ros_pi;
00200 moveFromPCL(clusters[i], ros_pi);
00201 ros_pi.header.stamp += ros::Duration (i * 0.001);
00202 pub_output_.publish (ros_pi);
00203 }
00204
00205 NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ());
00206 }
00207 else
00208 {
00209 for (size_t i = 0; i < clusters.size (); ++i)
00210 {
00211 if ((int)i >= max_clusters_)
00212 break;
00213 PointCloud output;
00214 copyPointCloud (*cloud, clusters[i].indices, output);
00215
00216
00217
00218
00219 std_msgs::Header header = fromPCL(output.header);
00220 header.stamp += ros::Duration (i * 0.001);
00221 toPCL(header, output.header);
00222
00223 pub_output_.publish (output.makeShared ());
00224 NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
00225 i, clusters[i].indices.size (), header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
00226 }
00227 }
00228 }
00229
00230 typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction;
00231 PLUGINLIB_DECLARE_CLASS (pcl, EuclideanClusterExtraction, EuclideanClusterExtraction, nodelet::Nodelet);
00232