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