diff --git a/pcl_ros/include/pcl_ros/features/feature.h b/pcl_ros/include/pcl_ros/features/feature.h index 3d5f9604..ca0dccc0 100644 --- a/pcl_ros/include/pcl_ros/features/feature.h +++ b/pcl_ros/include/pcl_ros/features/feature.h @@ -40,7 +40,7 @@ // PCL includes #include -#include +#include #include "pcl_ros/pcl_nodelet.h" #include diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h index 33050a41..8c24a889 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h +++ b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h @@ -93,7 +93,7 @@ namespace pcl_ros input_callback (const PointCloudConstPtr &input) { PointIndices cloud; - cloud.header.stamp = input->header.stamp; + cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp; nf_.add (boost::make_shared (cloud)); } diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 6257301a..30104d94 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -133,20 +133,20 @@ pcl_ros::Filter::onInit () if (approximate_sync_) { - sync_input_indices_a_ = boost::make_shared > >(max_queue_size_); + sync_input_indices_a_ = boost::make_shared > >(max_queue_size_); sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); } else { - sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); + sync_input_indices_e_ = boost::make_shared > >(max_queue_size_); sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); } } else // Subscribe in an old fashion to input only (no filters) - sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl::PointIndicesConstPtr ())); + sub_input_ = pnh_->subscribe ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ())); NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ()); } diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index 23ab6d12..7c54c414 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -40,6 +40,11 @@ #include "pcl_ros/segmentation/extract_polygonal_prism_data.h" #include +#include + +using pcl_conversions::fromPCL; +using pcl_conversions::toPCL; + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::ExtractPolygonalPrismData::onInit () @@ -120,14 +125,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( return; // Copy the header (stamp + frame_id) - pcl::PointIndices inliers; - inliers.header = cloud->header; + pcl_msgs::PointIndices inliers; + inliers.header = fromPCL(cloud->header); // If cloud is given, check if it's valid if (!isValid (cloud) || !isValid (hull, "planar_hull")) { NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ()); - pub_output_.publish (boost::make_shared (inliers)); + pub_output_.publish (boost::make_shared (inliers)); return; } // If indices are given, check if they are valid @@ -145,16 +150,16 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( " - 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 (), - hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").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 (), + hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").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_hull_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 (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (), - hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").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 (), + hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ()); /// if (cloud->header.frame_id != hull->header.frame_id) @@ -180,10 +185,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( impl_.setIndices (indices_ptr); // Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty ()) - impl_.segment (inliers); + if (!cloud->points.empty ()) { + pcl::PointIndices pcl_inliers; + toPCL(inliers, pcl_inliers); + impl_.segment (pcl_inliers); + fromPCL(pcl_inliers, inliers); + } // Enforce that the TF frame and the timestamp are copied - inliers.header = cloud->header; + inliers.header = fromPCL(cloud->header); pub_output_.publish (boost::make_shared (inliers)); NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ()); }