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
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050 #include <pcl/io/io.h>
00051 #include "pcl_ros/features/feature.h"
00052 #include <message_filters/null_types.h>
00053
00055 void
00056 pcl_ros::Feature::onInit ()
00057 {
00058
00059 PCLNodelet::onInit ();
00060
00061
00062 childInit (*pnh_);
00063
00064
00065
00066
00067
00068
00069 if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
00070 {
00071 NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
00072 return;
00073 }
00074 if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
00075 {
00076 NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
00077 return;
00078 }
00079
00080
00081 pnh_->getParam ("use_surface", use_surface_);
00082
00083
00084 srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
00085 dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&Feature::config_callback, this, _1, _2);
00086 srv_->setCallback (f);
00087
00088
00089 if (use_indices_ || use_surface_)
00090 {
00091
00092 if (approximate_sync_)
00093 sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
00094 else
00095 sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices> > >(max_queue_size_);
00096
00097
00098 sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00099 if (use_indices_)
00100 {
00101
00102 sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00103 if (use_surface_)
00104 {
00105
00106 sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00107 if (approximate_sync_)
00108 sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
00109 else
00110 sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, sub_indices_filter_);
00111 }
00112 else
00113 {
00114 sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
00115
00116 if (approximate_sync_)
00117 sync_input_surface_indices_a_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
00118 else
00119 sync_input_surface_indices_e_->connectInput (sub_input_filter_, nf_pc_, sub_indices_filter_);
00120 }
00121 }
00122 else
00123 {
00124 sub_input_filter_.registerCallback (bind (&Feature::input_callback, this, _1));
00125
00126 sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00127 if (approximate_sync_)
00128 sync_input_surface_indices_a_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
00129 else
00130 sync_input_surface_indices_e_->connectInput (sub_input_filter_, sub_surface_filter_, nf_pi_);
00131 }
00132
00133 if (approximate_sync_)
00134 sync_input_surface_indices_a_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
00135 else
00136 sync_input_surface_indices_e_->registerCallback (bind (&Feature::input_surface_indices_callback, this, _1, _2, _3));
00137 }
00138 else
00139
00140 sub_input_ = pnh_->subscribe<PointCloudIn> ("input", max_queue_size_, bind (&Feature::input_surface_indices_callback, this, _1, PointCloudInConstPtr (), PointIndicesConstPtr ()));
00141
00142 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00143 " - use_surface : %s\n"
00144 " - k_search : %d\n"
00145 " - radius_search : %f\n"
00146 " - spatial_locator: %d",
00147 getName ().c_str (),
00148 (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
00149 }
00150
00152 void
00153 pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level)
00154 {
00155 if (k_ != config.k_search)
00156 {
00157 k_ = config.k_search;
00158 NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
00159 }
00160 if (search_radius_ != config.radius_search)
00161 {
00162 search_radius_ = config.radius_search;
00163 NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_);
00164 }
00165 }
00166
00168 void
00169 pcl_ros::Feature::input_surface_indices_callback (const PointCloudInConstPtr &cloud,
00170 const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
00171 {
00172
00173 if (pub_output_.getNumSubscribers () <= 0)
00174 return;
00175
00176
00177 if (!isValid (cloud))
00178 {
00179 NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input!", getName ().c_str ());
00180 emptyPublish (cloud);
00181 return;
00182 }
00183
00184
00185 if (cloud_surface && !isValid (cloud_surface, "surface"))
00186 {
00187 NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input surface!", getName ().c_str ());
00188 emptyPublish (cloud);
00189 return;
00190 }
00191
00192
00193 if (indices && !isValid (indices))
00194 {
00195 NODELET_ERROR ("[%s::input_surface_indices_callback] Invalid input indices!", getName ().c_str ());
00196 emptyPublish (cloud);
00197 return;
00198 }
00199
00201 if (cloud_surface)
00202 if (indices)
00203 NODELET_DEBUG ("[input_surface_indices_callback]\n"
00204 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00205 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00206 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00207 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00208 cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
00209 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00210 else
00211 NODELET_DEBUG ("[input_surface_indices_callback]\n"
00212 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00213 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00214 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00215 cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str ());
00216
00217 else
00218 if (indices)
00219 NODELET_DEBUG ("[input_surface_indices_callback]\n"
00220 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00221 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00222 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00223 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00224 else
00225 NODELET_DEBUG ("[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
00227
00228
00229 if ((int)(cloud->width * cloud->height) < k_)
00230 {
00231 NODELET_ERROR ("[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_, (int)(cloud->width * cloud->height));
00232 emptyPublish (cloud);
00233 return;
00234 }
00235
00236
00237 IndicesPtr vindices;
00238 if (indices && !indices->header.frame_id.empty ())
00239 vindices.reset (new std::vector<int> (indices->indices));
00240
00241 computePublish (cloud, cloud_surface, vindices);
00242 }
00243
00247
00249 void
00250 pcl_ros::FeatureFromNormals::onInit ()
00251 {
00252
00253 PCLNodelet::onInit ();
00254
00255
00256 childInit (*pnh_);
00257
00258
00259
00260
00261
00262
00263 if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("radius_search", search_radius_))
00264 {
00265 NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
00266 return;
00267 }
00268 if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
00269 {
00270 NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
00271 return;
00272 }
00273
00274 pnh_->getParam ("use_surface", use_surface_);
00275
00276 sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00277 sub_normals_filter_.subscribe (*pnh_, "normals", max_queue_size_);
00278
00279
00280 srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
00281 dynamic_reconfigure::Server<FeatureConfig>::CallbackType f = boost::bind (&FeatureFromNormals::config_callback, this, _1, _2);
00282 srv_->setCallback (f);
00283
00284
00285 if (approximate_sync_)
00286 sync_input_normals_surface_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
00287 else
00288 sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (max_queue_size_);
00289
00290
00291 if (use_indices_ || use_surface_)
00292 {
00293 if (use_indices_)
00294 {
00295
00296 sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00297 if (use_surface_)
00298 {
00299
00300 sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00301 if (approximate_sync_)
00302 sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
00303 else
00304 sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, sub_indices_filter_);
00305 }
00306 else
00307 {
00308 sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
00309 if (approximate_sync_)
00310
00311 sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
00312 else
00313
00314 sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, sub_indices_filter_);
00315 }
00316 }
00317 else
00318 {
00319
00320 sub_surface_filter_.subscribe (*pnh_, "surface", max_queue_size_);
00321
00322 sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
00323 if (approximate_sync_)
00324 sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
00325 else
00326 sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, sub_surface_filter_, nf_pi_);
00327 }
00328 }
00329 else
00330 {
00331 sub_input_filter_.registerCallback (bind (&FeatureFromNormals::input_callback, this, _1));
00332
00333 if (approximate_sync_)
00334 sync_input_normals_surface_indices_a_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
00335 else
00336 sync_input_normals_surface_indices_e_->connectInput (sub_input_filter_, sub_normals_filter_, nf_pc_, nf_pi_);
00337 }
00338
00339
00340 if (approximate_sync_)
00341 sync_input_normals_surface_indices_a_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
00342 else
00343 sync_input_normals_surface_indices_e_->registerCallback (bind (&FeatureFromNormals::input_normals_surface_indices_callback, this, _1, _2, _3, _4));
00344
00345 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00346 " - use_surface : %s\n"
00347 " - k_search : %d\n"
00348 " - radius_search : %f\n"
00349 " - spatial_locator: %d",
00350 getName ().c_str (),
00351 (use_surface_) ? "true" : "false", k_, search_radius_, spatial_locator_type_);
00352 }
00353
00355 void
00356 pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback (
00357 const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals,
00358 const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
00359 {
00360
00361 if (pub_output_.getNumSubscribers () <= 0)
00362 return;
00363
00364
00365 if (!isValid (cloud))
00366 {
00367 NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ());
00368 emptyPublish (cloud);
00369 return;
00370 }
00371
00372
00373 if (cloud_surface && !isValid (cloud_surface, "surface"))
00374 {
00375 NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
00376 emptyPublish (cloud);
00377 return;
00378 }
00379
00380
00381 if (indices && !isValid (indices))
00382 {
00383 NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
00384 emptyPublish (cloud);
00385 return;
00386 }
00387
00389 if (cloud_surface)
00390 if (indices)
00391 NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00392 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00393 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00394 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00395 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00396 getName ().c_str (),
00397 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00398 cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
00399 cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
00400 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00401 else
00402 NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00403 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00404 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00405 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00406 getName ().c_str (),
00407 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00408 cloud_surface->width * cloud_surface->height, pcl::getFieldsList (*cloud_surface).c_str (), fromPCL(cloud_surface->header).stamp.toSec (), cloud_surface->header.frame_id.c_str (), pnh_->resolveName ("surface").c_str (),
00409 cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
00410 else
00411 if (indices)
00412 NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00413 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00414 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00415 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00416 getName ().c_str (),
00417 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00418 cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str (),
00419 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00420 else
00421 NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
00422 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00423 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00424 getName ().c_str (),
00425 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00426 cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), fromPCL(cloud_normals->header).stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").c_str ());
00428
00429 if ((int)(cloud->width * cloud->height) < k_)
00430 {
00431 NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", getName ().c_str (), k_, (int)(cloud->width * cloud->height));
00432 emptyPublish (cloud);
00433 return;
00434 }
00435
00436
00437 IndicesPtr vindices;
00438 if (indices && !indices->header.frame_id.empty ())
00439 vindices.reset (new std::vector<int> (indices->indices));
00440
00441 computePublish (cloud, cloud_normals, cloud_surface, vindices);
00442 }
00443