void
jsk_pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0)
return;
// If cloud is given, check if it's valid
if (!isValid (cloud))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
return;
}
/// DEBUG
if (indices)
NODELET_DEBUG ("[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
///
// Check whether the user has given a different input TF frame
tf_input_orig_frame_ = cloud->header.frame_id;
PointCloud2::ConstPtr cloud_tf;
if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
{
NODELET_DEBUG ("[%s::input_indices_callback] Transforming input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
// Save the original frame ID
// Convert the cloud into the different frame
PointCloud2 cloud_transformed;
tf_listener_.waitForTransform(cloud->header.frame_id, tf_input_frame_, cloud->header.stamp, ros::Duration(5.0));
if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_))
{
NODELET_ERROR ("[%s::input_indices_callback] Error converting input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
return;
}
cloud_tf = boost::make_shared<PointCloud2> (cloud_transformed);
}
else
cloud_tf = cloud;
// Need setInputCloud () here because we have to extract x/y/z
IndicesPtr vindices;
if (indices)
vindices.reset (new std::vector<int> (indices->indices));
computePublish (cloud_tf, vindices);
}
void
pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback (
const PointCloudInConstPtr &cloud, const PointCloudNConstPtr &cloud_normals,
const PointCloudInConstPtr &cloud_surface, const PointIndicesConstPtr &indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0)
return;
// If cloud+normals is given, check if it's valid
if (!isValid (cloud))// || !isValid (cloud_normals, "normals"))
{
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input!", getName ().c_str ());
emptyPublish (cloud);
return;
}
// If surface is given, check if it's valid
if (cloud_surface && !isValid (cloud_surface, "surface"))
{
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input surface!", getName ().c_str ());
emptyPublish (cloud);
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices))
{
NODELET_ERROR ("[%s::input_normals_surface_indices_callback] Invalid input indices!", getName ().c_str ());
emptyPublish (cloud);
return;
}
/// DEBUG
if (cloud_surface)
if (indices)
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
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 (),
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 (),
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 (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
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 (),
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 (),
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 ());
else
if (indices)
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
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 (),
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 (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
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 (),
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 ());
///
if ((int)(cloud->width * cloud->height) < k_)
{
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));
emptyPublish (cloud);
return;
}
// If indices given...
IndicesPtr vindices;
if (indices && !indices->header.frame_id.empty ())
vindices.reset (new std::vector<int> (indices->indices));
computePublish (cloud, cloud_normals, cloud_surface, vindices);
}
