From a3840127f8d37a04405292a2db3c83e4c17fb286 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 9 Jul 2013 18:56:23 -0700 Subject: [PATCH] Refactors to use pcl-1.7 --- .../include/pcl_ros/filters/extract_indices.h | 10 +++- pcl_ros/include/pcl_ros/filters/passthrough.h | 10 +++- .../include/pcl_ros/filters/project_inliers.h | 14 +++-- .../pcl_ros/filters/radius_outlier_removal.h | 10 +++- .../filters/statistical_outlier_removal.h | 10 +++- pcl_ros/include/pcl_ros/filters/voxel_grid.h | 2 +- .../include/pcl_ros/io/concatenate_fields.h | 2 + pcl_ros/include/pcl_ros/pcl_nodelet.h | 4 +- .../pcl_ros/segmentation/sac_segmentation.h | 8 +-- .../pcl_ros/surface/moving_least_squares.h | 2 +- pcl_ros/src/pcl_ros/filters/voxel_grid.cpp | 9 ++- pcl_ros/src/pcl_ros/io/concatenate_data.cpp | 2 + pcl_ros/src/pcl_ros/io/concatenate_fields.cpp | 2 + pcl_ros/src/pcl_ros/io/pcd_io.cpp | 9 ++- .../pcl_ros/segmentation/extract_clusters.cpp | 34 +++++++---- .../extract_polygonal_prism_data.cpp | 16 +++--- .../pcl_ros/segmentation/sac_segmentation.cpp | 56 ++++++++++++------- .../segmentation/segment_differences.cpp | 6 +- pcl_ros/src/pcl_ros/surface/convex_hull.cpp | 6 +- .../pcl_ros/surface/moving_least_squares.cpp | 4 +- pcl_ros/tools/pointcloud_to_pcd.cpp | 7 ++- 21 files changed, 147 insertions(+), 76 deletions(-) diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.h b/pcl_ros/include/pcl_ros/filters/extract_indices.h index 01cfde83..cdb60e8f 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.h +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.h @@ -66,9 +66,13 @@ namespace pcl_ros PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.filter (output); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. @@ -84,7 +88,7 @@ namespace pcl_ros private: /** \brief The PCL filter implementation used. */ - pcl::ExtractIndices impl_; + pcl::ExtractIndices impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.h b/pcl_ros/include/pcl_ros/filters/passthrough.h index c21a7d1b..7d6c9649 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.h +++ b/pcl_ros/include/pcl_ros/filters/passthrough.h @@ -64,9 +64,13 @@ namespace pcl_ros PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL (*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.filter (output); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. @@ -85,7 +89,7 @@ namespace pcl_ros private: /** \brief The PCL filter implementation used. */ - pcl::PassThrough impl_; + pcl::PassThrough impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.h b/pcl_ros/include/pcl_ros/filters/project_inliers.h index 200ef10e..636291fe 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.h +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.h @@ -68,10 +68,16 @@ namespace pcl_ros filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL (*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.setModelCoefficients (model_); - impl_.filter (output); + pcl::ModelCoefficients::Ptr pcl_model; + pcl_conversions::toPCL(*(model_), *(pcl_model)); + impl_.setModelCoefficients (pcl_model); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } private: @@ -85,7 +91,7 @@ namespace pcl_ros boost::shared_ptr > > sync_input_indices_model_e_; boost::shared_ptr > > sync_input_indices_model_a_; /** \brief The PCL filter implementation used. */ - pcl::ProjectInliers impl_; + pcl::ProjectInliers impl_; /** \brief Nodelet initialization routine. */ virtual void diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h index fc316a83..38babf0c 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h @@ -61,9 +61,13 @@ namespace pcl_ros filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, PointCloud2 &output) { - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL (*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.filter (output); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. @@ -73,7 +77,7 @@ namespace pcl_ros private: /** \brief The PCL filter implementation used. */ - pcl::RadiusOutlierRemoval impl_; + pcl::RadiusOutlierRemoval impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h index e97735d5..22ed8b82 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h @@ -74,9 +74,13 @@ namespace pcl_ros PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.filter (output); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } /** \brief Child initialization routine. @@ -93,7 +97,7 @@ namespace pcl_ros private: /** \brief The PCL filter implementation used. */ - pcl::StatisticalOutlierRemoval impl_; + pcl::StatisticalOutlierRemoval impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.h b/pcl_ros/include/pcl_ros/filters/voxel_grid.h index 95942259..1d887f7e 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.h +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.h @@ -57,7 +57,7 @@ namespace pcl_ros boost::shared_ptr > srv_; /** \brief The PCL filter implementation used. */ - pcl::VoxelGrid impl_; + pcl::VoxelGrid impl_; /** \brief Call the actual filter. * \param input the input point cloud dataset diff --git a/pcl_ros/include/pcl_ros/io/concatenate_fields.h b/pcl_ros/include/pcl_ros/io/concatenate_fields.h index f6252b68..13a9ced3 100644 --- a/pcl_ros/include/pcl_ros/io/concatenate_fields.h +++ b/pcl_ros/include/pcl_ros/io/concatenate_fields.h @@ -45,6 +45,8 @@ #include #include +#include + namespace pcl_ros { /** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.h b/pcl_ros/include/pcl_ros/pcl_nodelet.h index dbb8b460..ba69afdc 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.h +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.h @@ -47,7 +47,7 @@ #include // PCL includes #include -#include +#include #include #include #include "pcl_ros/point_cloud.h" @@ -82,7 +82,7 @@ namespace pcl_ros typedef PointIndices::Ptr PointIndicesPtr; typedef PointIndices::ConstPtr PointIndicesConstPtr; - typedef pcl::ModelCoefficients ModelCoefficients; + typedef pcl_msgs::ModelCoefficients ModelCoefficients; typedef ModelCoefficients::Ptr ModelCoefficientsPtr; typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h index 7ff369b6..fc10d104 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h @@ -112,7 +112,7 @@ namespace pcl_ros /** \brief Null passthrough filter, used for pushing empty elements in the * synchronizer */ - message_filters::PassThrough nf_pi_; + message_filters::PassThrough nf_pi_; /** \brief Nodelet initialization routine. */ virtual void onInit (); @@ -149,7 +149,7 @@ namespace pcl_ros inline void input_callback (const PointCloudConstPtr &input) { - indices_.header = input->header; + indices_.header = fromPCL(input->header); PointIndicesConstPtr indices; indices.reset (new PointIndices (indices_)); nf_pi_.add (indices); @@ -220,7 +220,7 @@ namespace pcl_ros input_callback (const PointCloudConstPtr &cloud) { PointIndices indices; - indices.header.stamp = cloud->header.stamp; + indices.header.stamp = fromPCL(cloud->header).stamp; nf_.add (boost::make_shared (indices)); } @@ -241,7 +241,7 @@ namespace pcl_ros /** \brief Model callback * \param model the sample consensus model found */ - void axis_callback (const pcl::ModelCoefficientsConstPtr &model); + void axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model); /** \brief Dynamic reconfigure callback * \param config the config object diff --git a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h index 87aa2662..656a52a9 100644 --- a/pcl_ros/include/pcl_ros/surface/moving_least_squares.h +++ b/pcl_ros/include/pcl_ros/surface/moving_least_squares.h @@ -59,7 +59,7 @@ namespace pcl_ros class MovingLeastSquares : public PCLNodelet { typedef pcl::PointXYZ PointIn; - typedef pcl::Normal NormalOut; + typedef pcl::PointNormal NormalOut; typedef pcl::PointCloud PointCloudIn; typedef PointCloudIn::Ptr PointCloudInPtr; diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index 87c6ea43..6405b3f4 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -58,10 +58,13 @@ pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input, PointCloud2 &output) { boost::mutex::scoped_lock lock (mutex_); - - impl_.setInputCloud (input); + pcl::PCLPointCloud2::Ptr pcl_input; + pcl_conversions::toPCL (*(input), *(pcl_input)); + impl_.setInputCloud (pcl_input); impl_.setIndices (indices); - impl_.filter (output); + pcl::PCLPointCloud2 pcl_output; + impl_.filter (pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp index e4ccba9c..90000feb 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_data.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_data.cpp @@ -40,6 +40,8 @@ #include "pcl_ros/transforms.h" #include "pcl_ros/io/concatenate_data.h" +#include + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::PointCloudConcatenateDataSynchronizer::onInit () diff --git a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp index 52b17edd..ea64c1f6 100644 --- a/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp +++ b/pcl_ros/src/pcl_ros/io/concatenate_fields.cpp @@ -41,6 +41,8 @@ #include #include "pcl_ros/io/concatenate_fields.h" +#include + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit () diff --git a/pcl_ros/src/pcl_ros/io/pcd_io.cpp b/pcl_ros/src/pcl_ros/io/pcd_io.cpp index 026c0d20..07d6bfae 100644 --- a/pcl_ros/src/pcl_ros/io/pcd_io.cpp +++ b/pcl_ros/src/pcl_ros/io/pcd_io.cpp @@ -86,13 +86,13 @@ pcl_ros::PCDReader::onInit () { NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ()); file_name = file_name_; - PointCloud2 cloud; + pcl::PCLPointCloud2 cloud; if (impl_.read (file_name_, cloud) < 0) { NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ()); return; } - output_ = boost::make_shared (cloud); + pcl_conversions::moveFromPCL(cloud, *(output_)); output_->header.stamp = ros::Time::now (); output_->header.frame_id = tf_frame_; } @@ -163,7 +163,10 @@ pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud) fname = boost::lexical_cast (cloud->header.stamp.toSec ()) + ".pcd"; else fname = file_name_; - impl_.write (fname, cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_); + pcl::PCLPointCloud2 pcl_cloud; + // It is safe to remove the const here because we are the only subscriber callback. + pcl_conversions::moveToPCL(*(const_cast(cloud.get())), pcl_cloud); + impl_.write (fname, pcl_cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_); NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ()); } diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index 0bff57bf..686c0006 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -37,8 +37,15 @@ #include #include +#include #include "pcl_ros/segmentation/extract_clusters.h" +#include + +using pcl_conversions::fromPCL; +using pcl_conversions::moveFromPCL; +using pcl_conversions::toPCL; + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::EuclideanClusterExtraction::onInit () @@ -158,15 +165,18 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( } /// DEBUG - if (indices) + if (indices) { + std_msgs::Header cloud_header = fromPCL(cloud->header); + std_msgs::Header indices_header = indices->header; 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_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 ()); + 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_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); + } /// IndicesPtr indices_ptr; @@ -176,7 +186,7 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( impl_.setInputCloud (cloud); impl_.setIndices (indices_ptr); - std::vector clusters; + std::vector clusters; impl_.extract (clusters); if (publish_indices_) @@ -186,8 +196,10 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( if ((int)i >= max_clusters_) break; // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. - clusters[i].header.stamp += ros::Duration (i * 0.001); - pub_output_.publish (boost::make_shared (clusters[i])); + pcl_msgs::PointIndices ros_pi; + moveFromPCL(clusters[i], ros_pi); + ros_pi.header.stamp += ros::Duration (i * 0.001); + pub_output_.publish (ros_pi); } NODELET_DEBUG ("[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (), pnh_->resolveName ("output").c_str ()); @@ -204,11 +216,13 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback ( //PointCloud output_blob; // Convert from the templated output to the PointCloud blob //pcl::toROSMsg (output, output_blob); // TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. - output.header.stamp += ros::Duration (i * 0.001); + std_msgs::Header header = fromPCL(output.header); + header.stamp += ros::Duration (i * 0.001); + toPCL(header, output.header); // Publish a Boost shared ptr const data pub_output_.publish (output.makeShared ()); NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s", - i, clusters[i].indices.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ()); + i, clusters[i].indices.size (), header.stamp.toSec (), pnh_->resolveName ("output").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 7c54c414..b75b3ecd 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 @@ -42,8 +42,8 @@ #include -using pcl_conversions::fromPCL; -using pcl_conversions::toPCL; +using pcl_conversions::moveFromPCL; +using pcl_conversions::moveToPCL; ////////////////////////////////////////////////////////////////////////////////////////////// void @@ -132,14 +132,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( 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 (inliers); return; } // If indices are given, check if they are valid if (indices && !isValid (indices)) { NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ()); - pub_output_.publish (boost::make_shared (inliers)); + pub_output_.publish (inliers); return; } @@ -169,7 +169,7 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_)) { // Publish empty before return - pub_output_.publish (boost::make_shared (inliers)); + pub_output_.publish (inliers); return; } impl_.setInputPlanarHull (planar_hull.makeShared ()); @@ -187,13 +187,13 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback ( // 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 ()) { pcl::PointIndices pcl_inliers; - toPCL(inliers, pcl_inliers); + moveToPCL(inliers, pcl_inliers); impl_.segment (pcl_inliers); - fromPCL(pcl_inliers, inliers); + moveFromPCL(pcl_inliers, inliers); } // Enforce that the TF frame and the timestamp are copied inliers.header = fromPCL(cloud->header); - pub_output_.publish (boost::make_shared (inliers)); + pub_output_.publish (inliers); NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ()); } diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index 8478c71d..fcefc3bc 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -39,6 +39,10 @@ #include "pcl_ros/segmentation/sac_segmentation.h" #include +#include + +using pcl_conversions::fromPCL; + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SACSegmentation::onInit () @@ -249,25 +253,25 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0) return; - PointIndices inliers; - ModelCoefficients model; + pcl_msgs::PointIndices inliers; + pcl_msgs::ModelCoefficients model; // Enforce that the TF frame and the timestamp are copied - inliers.header = model.header = cloud->header; + inliers.header = model.header = fromPCL(cloud->header); // If cloud is given, check if it's valid if (!isValid (cloud)) { NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + pub_indices_.publish (inliers); + pub_model_.publish (model); 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 ()); - pub_indices_.publish (boost::make_shared (inliers)); - pub_model_.publish (boost::make_shared (model)); + pub_indices_.publish (inliers); + pub_model_.publish (model); return; } @@ -277,11 +281,11 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou " - 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 (), + 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 (), 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, 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 ()); + getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ()); /// // Check whether the user has given a different input TF frame @@ -308,8 +312,15 @@ pcl_ros::SACSegmentation::input_indices_callback (const PointCloudConstPtr &clou 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, model); + if (!cloud->points.empty ()) { + pcl::PointIndices pcl_inliers; + pcl::ModelCoefficients pcl_model; + pcl_conversions::moveToPCL(inliers, pcl_inliers); + pcl_conversions::moveToPCL(model, pcl_model); + impl_.segment (pcl_inliers, pcl_model); + pcl_conversions::moveFromPCL(pcl_inliers, inliers); + pcl_conversions::moveFromPCL(pcl_model, model); + } // Probably need to transform the model of the plane here @@ -453,7 +464,7 @@ pcl_ros::SACSegmentationFromNormals::onInit () ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl::ModelCoefficientsConstPtr &model) +pcl_ros::SACSegmentationFromNormals::axis_callback (const pcl_msgs::ModelCoefficientsConstPtr &model) { boost::mutex::scoped_lock lock (mutex_); @@ -551,7 +562,7 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback ( PointIndices inliers; ModelCoefficients model; // Enforce that the TF frame and the timestamp are copied - inliers.header = model.header = cloud->header; + inliers.header = model.header = fromPCL(cloud->header); if (impl_.getModelType () < 0) { @@ -584,16 +595,16 @@ pcl_ros::SACSegmentationFromNormals::input_normals_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 (), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList (*cloud_normals).c_str (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").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_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 (), 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 (), cloud_normals->header.stamp.toSec (), cloud_normals->header.frame_id.c_str (), pnh_->resolveName ("normals").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 ()); /// @@ -618,8 +629,15 @@ pcl_ros::SACSegmentationFromNormals::input_normals_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, model); + if (!cloud->points.empty ()) { + pcl::PointIndices pcl_inliers; + pcl::ModelCoefficients pcl_model; + pcl_conversions::moveToPCL(inliers, pcl_inliers); + pcl_conversions::moveToPCL(model, pcl_model); + impl_.segment (pcl_inliers, pcl_model); + pcl_conversions::moveFromPCL(pcl_inliers, inliers); + pcl_conversions::moveFromPCL(pcl_model, model); + } // Check if we have enough inliers, clear inliers + model if not if ((int)inliers.indices.size () <= min_inliers_) diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp index f8fbae11..eceb5b17 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -109,8 +109,8 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl " - 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 (), - cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), cloud_target->header.stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").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_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), fromPCL(cloud_target->header).stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ()); impl_.setInputCloud (cloud); impl_.setTargetCloud (cloud_target); @@ -120,7 +120,7 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl pub_output_.publish (output.makeShared ()); NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (), - output.points.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ()); + output.points.size (), fromPCL(output.header).stamp.toSec (), pnh_->resolveName ("output").c_str ()); } typedef pcl_ros::SegmentDifferences SegmentDifferences; diff --git a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp index 94dc5336..97417e58 100644 --- a/pcl_ros/src/pcl_ros/surface/convex_hull.cpp +++ b/pcl_ros/src/pcl_ros/surface/convex_hull.cpp @@ -120,10 +120,10 @@ void " - 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 (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ()); else - NODELET_DEBUG ("[%s::input_indices_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 (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); + NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); // Reset the indices and surface pointers IndicesPtr indices_ptr; @@ -140,7 +140,7 @@ void if (output.points.size () >= 3) { geometry_msgs::PolygonStamped poly; - poly.header = output.header; + poly.header = fromPCL(output.header); poly.polygon.points.resize (output.points.size ()); // Get three consecutive points (without copying) pcl::Vector4fMap O = output.points[1].getVector4fMap (); diff --git a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp index 2cb41cff..8f3caf5f 100644 --- a/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp +++ b/pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp @@ -141,10 +141,10 @@ pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr " - 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 (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), + cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (), indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ()); else - 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 (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); + 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, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ()); /// // Reset the indices and surface pointers diff --git a/pcl_ros/tools/pointcloud_to_pcd.cpp b/pcl_ros/tools/pointcloud_to_pcd.cpp index 9b52113c..00e92734 100644 --- a/pcl_ros/tools/pointcloud_to_pcd.cpp +++ b/pcl_ros/tools/pointcloud_to_pcd.cpp @@ -37,11 +37,16 @@ // ROS core #include + +#include + // PCL includes #include #include #include +#include + using namespace std; /** @@ -67,7 +72,7 @@ class PointCloudToPCD //////////////////////////////////////////////////////////////////////////////// // Callback void - cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud) + cloud_cb (const pcl::PCLPointCloud2::ConstPtr& cloud) { if ((cloud->width * cloud->height) == 0) return;