50 #include <pcl/io/io.h> 71 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 ());
76 NODELET_ERROR (
"[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName ().c_str ());
84 srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
86 srv_->setCallback (f);
88 NODELET_DEBUG (
"[%s::onInit] Nodelet successfully created with the following parameters:\n" 89 " - use_surface : %s\n" 91 " - radius_search : %f\n" 92 " - spatial_locator: %d",
182 if (
k_ != config.k_search)
184 k_ = config.k_search;
185 NODELET_DEBUG (
"[config_callback] Setting the number of K nearest neighbors to use for each point: %d.",
k_);
212 if (cloud_surface && !
isValid (cloud_surface,
"surface"))
214 NODELET_ERROR (
"[%s::input_surface_indices_callback] Invalid input surface!",
getName ().c_str ());
220 if (indices && !
isValid (indices))
222 NODELET_ERROR (
"[%s::input_surface_indices_callback] Invalid input indices!",
getName ().c_str ());
231 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 232 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 233 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
234 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 (),
235 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 (),
236 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (),
pnh_->resolveName (
"indices").c_str ());
239 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 240 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
241 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 (),
242 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 ());
247 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 248 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
249 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 (),
250 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (),
pnh_->resolveName (
"indices").c_str ());
252 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 ());
256 if ((
int)(cloud->width * cloud->height) <
k_)
258 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));
265 if (indices && !indices->header.frame_id.empty ())
266 vindices.reset (
new std::vector<int> (indices->indices));
292 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 ());
297 NODELET_ERROR (
"[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName ().c_str ());
304 srv_ = boost::make_shared<dynamic_reconfigure::Server<FeatureConfig> > (*pnh_);
306 srv_->setCallback (f);
308 NODELET_DEBUG (
"[%s::onInit] Nodelet successfully created with the following parameters:\n" 309 " - use_surface : %s\n" 311 " - radius_search : %f\n" 312 " - spatial_locator: %d",
328 sync_input_normals_surface_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (
max_queue_size_);
330 sync_input_normals_surface_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn, PointCloudN, PointCloudIn, PointIndices> > > (
max_queue_size_);
393 sub_normals_filter_.unsubscribe ();
420 NODELET_ERROR (
"[%s::input_normals_surface_indices_callback] Invalid input!",
getName ().c_str ());
426 if (cloud_surface && !
isValid (cloud_surface,
"surface"))
428 NODELET_ERROR (
"[%s::input_normals_surface_indices_callback] Invalid input surface!",
getName ().c_str ());
434 if (indices && !
isValid (indices))
436 NODELET_ERROR (
"[%s::input_normals_surface_indices_callback] Invalid input indices!",
getName ().c_str ());
444 NODELET_DEBUG (
"[%s::input_normals_surface_indices_callback]\n" 445 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 446 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 447 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 448 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
450 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 (),
451 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 (),
452 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 (),
453 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (),
pnh_->resolveName (
"indices").c_str ());
455 NODELET_DEBUG (
"[%s::input_normals_surface_indices_callback]\n" 456 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 457 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 458 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
460 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 (),
461 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 (),
462 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 ());
465 NODELET_DEBUG (
"[%s::input_normals_surface_indices_callback]\n" 466 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 467 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 468 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
470 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 (),
471 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 (),
472 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (),
pnh_->resolveName (
"indices").c_str ());
474 NODELET_DEBUG (
"[%s::input_normals_surface_indices_callback]\n" 475 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 476 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
478 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 (),
479 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 ());
482 if ((
int)(cloud->width * cloud->height) <
k_)
484 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));
491 if (indices && !indices->header.frame_id.empty ())
492 vindices.reset (
new std::vector<int> (indices->indices));
virtual void emptyPublish(const PointCloudInConstPtr &cloud)=0
Publish an empty point cloud of the feature output type.
virtual void onInit()
Nodelet initialization routine.
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
virtual void subscribe()
NodeletLazy connection routine.
#define NODELET_ERROR(...)
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
virtual void onInit()
Nodelet initialization routine.
bool use_surface_
Set to true if the nodelet needs to listen for incoming point clouds representing the search surface...
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloudIn, PointCloudIn, PointIndices > > > sync_input_surface_indices_a_
Synchronized input, surface, and point indices.
const std::string & getName() const
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloudIn, PointCloudIn, PointIndices > > > sync_input_surface_indices_e_
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
void config_callback(FeatureConfig &config, uint32_t level)
Dynamic reconfigure callback.
message_filters::PassThrough< PointCloudIn > nf_pc_
void input_normals_surface_indices_callback(const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals, const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
Input point cloud callback. Used when use_indices and use_surface are set.
bool isValid(const PointCloud2::ConstPtr &cloud, const std::string &topic_name="input")
Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-ze...
int k_
The number of K nearest neighbors to use for each point.
PointCloudN::ConstPtr PointCloudNConstPtr
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
double search_radius_
The nearest neighbors search radius for each point.
virtual void subscribe()
NodeletLazy connection routine.
boost::shared_ptr< dynamic_reconfigure::Server< FeatureConfig > > srv_
Pointer to a dynamic reconfigure service.
pcl::PointCloud< pcl::PointXYZ > PointCloudIn
void input_surface_indices_callback(const PointCloudInConstPtr &cloud, const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
Input point cloud callback. Used when use_indices and use_surface are set.
message_filters::PassThrough< PointIndices > nf_pi_
Null passthrough filter, used for pushing empty elements in the synchronizer.
message_filters::Subscriber< PointCloudIn > sub_surface_filter_
The surface PointCloud subscriber filter.
ros::Subscriber sub_input_
The input PointCloud subscriber.
uint32_t getNumSubscribers() const
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
int spatial_locator_type_
Parameter for the spatial locator tree. By convention, the values represent: 0: ANN (Approximate Near...
bool approximate_sync_
True if we use an approximate time synchronizer versus an exact one (false by default).
int max_queue_size_
The maximum queue size (default: 3).
virtual void unsubscribe()
ros::Publisher pub_output_
The output PointCloud publisher.
PointCloudIn::ConstPtr PointCloudInConstPtr
virtual void computePublish(const PointCloudInConstPtr &cloud, const PointCloudInConstPtr &surface, const IndicesPtr &indices)=0
Compute the feature and publish it. Internal method.
#define NODELET_DEBUG(...)
bool use_indices_
Set to true if point indices are used.
PointIndices::ConstPtr PointIndicesConstPtr
virtual bool childInit(ros::NodeHandle &nh)=0
Child initialization routine. Internal method.
virtual void unsubscribe()
message_filters::Subscriber< PointCloud > sub_input_filter_
The message filter subscriber for PointCloud2.
void input_callback(const PointCloudInConstPtr &input)
Input point cloud callback. Because we want to use the same synchronizer object, we push back empty e...
Connection registerCallback(const C &callback)