apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Refactors to use pcl-1.7

Browse Source
This commit is contained in:
William Woodall 2013-07-09 18:56:23 -07:00 committed by Paul Bovbel
parent 4e64cb25e7
commit a3840127f8
21 changed files with 147 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
pcl_ros/include/pcl_ros/filters/extract_indices.h
View File

@ -66,9 +66,13 @@ namespace pcl_ros
PointCloud2 &output) PointCloud2 &output)
{ {
boost::mutex::scoped_lock lock (mutex_); 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_.setIndices (indices);
impl_.filter (output); pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
} }
/** \brief Child initialization routine. /** \brief Child initialization routine.
@ -84,7 +88,7 @@ namespace pcl_ros
private: private:
/** \brief The PCL filter implementation used. */ /** \brief The PCL filter implementation used. */
pcl::ExtractIndices<sensor_msgs::PointCloud2> impl_; pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };

10
pcl_ros/include/pcl_ros/filters/passthrough.h
View File

@ -64,9 +64,13 @@ namespace pcl_ros
PointCloud2 &output) PointCloud2 &output)
{ {
boost::mutex::scoped_lock lock (mutex_); 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_.setIndices (indices);
impl_.filter (output); pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
} }
/** \brief Child initialization routine. /** \brief Child initialization routine.
@ -85,7 +89,7 @@ namespace pcl_ros
private: private:
/** \brief The PCL filter implementation used. */ /** \brief The PCL filter implementation used. */
pcl::PassThrough<sensor_msgs::PointCloud2> impl_; pcl::PassThrough<pcl::PCLPointCloud2> impl_;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };

14
pcl_ros/include/pcl_ros/filters/project_inliers.h
View File

@ -68,10 +68,16 @@ namespace pcl_ros
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output) PointCloud2 &output)
{ {
impl_.setInputCloud (input); pcl::PCLPointCloud2::Ptr pcl_input;
pcl_conversions::toPCL (*(input), *(pcl_input));
impl_.setInputCloud (pcl_input);
impl_.setIndices (indices); impl_.setIndices (indices);
impl_.setModelCoefficients (model_); pcl::ModelCoefficients::Ptr pcl_model;
impl_.filter (output); 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: private:
@ -85,7 +91,7 @@ namespace pcl_ros
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_e_; boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_a_; boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > sync_input_indices_model_a_;
/** \brief The PCL filter implementation used. */ /** \brief The PCL filter implementation used. */
pcl::ProjectInliers<PointCloud2> impl_; pcl::ProjectInliers<pcl::PCLPointCloud2> impl_;
/** \brief Nodelet initialization routine. */ /** \brief Nodelet initialization routine. */
virtual void virtual void

10
pcl_ros/include/pcl_ros/filters/radius_outlier_removal.h
View File

@ -61,9 +61,13 @@ namespace pcl_ros
filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices, filter (const PointCloud2::ConstPtr &input, const IndicesPtr &indices,
PointCloud2 &output) PointCloud2 &output)
{ {
impl_.setInputCloud (input); pcl::PCLPointCloud2::Ptr pcl_input;
pcl_conversions::toPCL (*(input), *(pcl_input));
impl_.setInputCloud (pcl_input);
impl_.setIndices (indices); impl_.setIndices (indices);
impl_.filter (output); pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
} }
/** \brief Child initialization routine. /** \brief Child initialization routine.
@ -73,7 +77,7 @@ namespace pcl_ros
private: private:
/** \brief The PCL filter implementation used. */ /** \brief The PCL filter implementation used. */
pcl::RadiusOutlierRemoval<sensor_msgs::PointCloud2> impl_; pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> impl_;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };

10
pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.h
View File

@ -74,9 +74,13 @@ namespace pcl_ros
PointCloud2 &output) PointCloud2 &output)
{ {
boost::mutex::scoped_lock lock (mutex_); 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_.setIndices (indices);
impl_.filter (output); pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
} }
/** \brief Child initialization routine. /** \brief Child initialization routine.
@ -93,7 +97,7 @@ namespace pcl_ros
private: private:
/** \brief The PCL filter implementation used. */ /** \brief The PCL filter implementation used. */
pcl::StatisticalOutlierRemoval<sensor_msgs::PointCloud2> impl_; pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> impl_;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };

2
pcl_ros/include/pcl_ros/filters/voxel_grid.h
View File

@ -57,7 +57,7 @@ namespace pcl_ros
boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > srv_; boost::shared_ptr <dynamic_reconfigure::Server<pcl_ros::VoxelGridConfig> > srv_;
/** \brief The PCL filter implementation used. */ /** \brief The PCL filter implementation used. */
pcl::VoxelGrid<sensor_msgs::PointCloud2> impl_; pcl::VoxelGrid<pcl::PCLPointCloud2> impl_;
/** \brief Call the actual filter. /** \brief Call the actual filter.
* \param input the input point cloud dataset * \param input the input point cloud dataset

2
pcl_ros/include/pcl_ros/io/concatenate_fields.h
View File

@ -45,6 +45,8 @@
#include <message_filters/sync_policies/exact_time.h> #include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h> #include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/PointCloud2.h>
namespace pcl_ros namespace pcl_ros
{ {
/** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of /** \brief @b PointCloudConcatenateFieldsSynchronizer is a special form of data synchronizer: it listens for a set of

4
pcl_ros/include/pcl_ros/pcl_nodelet.h
View File

@ -47,7 +47,7 @@
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
// PCL includes // PCL includes
#include <pcl_msgs/PointIndices.h> #include <pcl_msgs/PointIndices.h>
#include <pcl/ModelCoefficients.h> #include <pcl_msgs/ModelCoefficients.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include "pcl_ros/point_cloud.h" #include "pcl_ros/point_cloud.h"
@ -82,7 +82,7 @@ namespace pcl_ros
typedef PointIndices::Ptr PointIndicesPtr; typedef PointIndices::Ptr PointIndicesPtr;
typedef PointIndices::ConstPtr PointIndicesConstPtr; typedef PointIndices::ConstPtr PointIndicesConstPtr;
typedef pcl::ModelCoefficients ModelCoefficients; typedef pcl_msgs::ModelCoefficients ModelCoefficients;
typedef ModelCoefficients::Ptr ModelCoefficientsPtr; typedef ModelCoefficients::Ptr ModelCoefficientsPtr;
typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr;

8
pcl_ros/include/pcl_ros/segmentation/sac_segmentation.h
View File

@ -112,7 +112,7 @@ namespace pcl_ros
/** \brief Null passthrough filter, used for pushing empty elements in the /** \brief Null passthrough filter, used for pushing empty elements in the
* synchronizer */ * synchronizer */
message_filters::PassThrough<PointIndices> nf_pi_; message_filters::PassThrough<pcl_msgs::PointIndices> nf_pi_;
/** \brief Nodelet initialization routine. */ /** \brief Nodelet initialization routine. */
virtual void onInit (); virtual void onInit ();
@ -149,7 +149,7 @@ namespace pcl_ros
inline void inline void
input_callback (const PointCloudConstPtr &input) input_callback (const PointCloudConstPtr &input)
{ {
indices_.header = input->header; indices_.header = fromPCL(input->header);
PointIndicesConstPtr indices; PointIndicesConstPtr indices;
indices.reset (new PointIndices (indices_)); indices.reset (new PointIndices (indices_));
nf_pi_.add (indices); nf_pi_.add (indices);
@ -220,7 +220,7 @@ namespace pcl_ros
input_callback (const PointCloudConstPtr &cloud) input_callback (const PointCloudConstPtr &cloud)
{ {
PointIndices indices; PointIndices indices;
indices.header.stamp = cloud->header.stamp; indices.header.stamp = fromPCL(cloud->header).stamp;
nf_.add (boost::make_shared<PointIndices> (indices)); nf_.add (boost::make_shared<PointIndices> (indices));
} }
@ -241,7 +241,7 @@ namespace pcl_ros
/** \brief Model callback /** \brief Model callback
* \param model the sample consensus model found * \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 /** \brief Dynamic reconfigure callback
* \param config the config object * \param config the config object

2
pcl_ros/include/pcl_ros/surface/moving_least_squares.h
View File

@ -59,7 +59,7 @@ namespace pcl_ros
class MovingLeastSquares : public PCLNodelet class MovingLeastSquares : public PCLNodelet
{ {
typedef pcl::PointXYZ PointIn; typedef pcl::PointXYZ PointIn;
typedef pcl::Normal NormalOut; typedef pcl::PointNormal NormalOut;
typedef pcl::PointCloud<PointIn> PointCloudIn; typedef pcl::PointCloud<PointIn> PointCloudIn;
typedef PointCloudIn::Ptr PointCloudInPtr; typedef PointCloudIn::Ptr PointCloudInPtr;

9
pcl_ros/src/pcl_ros/filters/voxel_grid.cpp
View File

@ -58,10 +58,13 @@ pcl_ros::VoxelGrid::filter (const PointCloud2::ConstPtr &input,
PointCloud2 &output) PointCloud2 &output)
{ {
boost::mutex::scoped_lock lock (mutex_); boost::mutex::scoped_lock lock (mutex_);
pcl::PCLPointCloud2::Ptr pcl_input;
impl_.setInputCloud (input); pcl_conversions::toPCL (*(input), *(pcl_input));
impl_.setInputCloud (pcl_input);
impl_.setIndices (indices); impl_.setIndices (indices);
impl_.filter (output); pcl::PCLPointCloud2 pcl_output;
impl_.filter (pcl_output);
pcl_conversions::moveFromPCL(pcl_output, output);
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////

2
pcl_ros/src/pcl_ros/io/concatenate_data.cpp
View File

@ -40,6 +40,8 @@
#include "pcl_ros/transforms.h" #include "pcl_ros/transforms.h"
#include "pcl_ros/io/concatenate_data.h" #include "pcl_ros/io/concatenate_data.h"
#include <pcl_conversions/pcl_conversions.h>
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateDataSynchronizer::onInit () pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()

2
pcl_ros/src/pcl_ros/io/concatenate_fields.cpp
View File

@ -41,6 +41,8 @@
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include "pcl_ros/io/concatenate_fields.h" #include "pcl_ros/io/concatenate_fields.h"
#include <pcl_conversions/pcl_conversions.h>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit () pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit ()

9
pcl_ros/src/pcl_ros/io/pcd_io.cpp
View File

@ -86,13 +86,13 @@ pcl_ros::PCDReader::onInit ()
{ {
NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ()); NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ());
file_name = file_name_; file_name = file_name_;
PointCloud2 cloud; pcl::PCLPointCloud2 cloud;
if (impl_.read (file_name_, cloud) < 0) if (impl_.read (file_name_, cloud) < 0)
{ {
NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ()); NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ());
return; return;
} }
output_ = boost::make_shared<PointCloud2> (cloud); pcl_conversions::moveFromPCL(cloud, *(output_));
output_->header.stamp = ros::Time::now (); output_->header.stamp = ros::Time::now ();
output_->header.frame_id = tf_frame_; output_->header.frame_id = tf_frame_;
} }
@ -163,7 +163,10 @@ pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud)
fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd"; fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd";
else else
fname = file_name_; 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<PointCloud2*>(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 ()); NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ());
} }

34
pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp
View File

@ -37,8 +37,15 @@
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <pcl/PointIndices.h>
#include "pcl_ros/segmentation/extract_clusters.h" #include "pcl_ros/segmentation/extract_clusters.h"
#include <pcl_conversions/pcl_conversions.h>
using pcl_conversions::fromPCL;
using pcl_conversions::moveFromPCL;
using pcl_conversions::toPCL;
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::EuclideanClusterExtraction::onInit () pcl_ros::EuclideanClusterExtraction::onInit ()
@ -158,15 +165,18 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
} }
/// DEBUG /// 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" NODELET_DEBUG ("[%s::input_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.", " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (), 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 (), 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 ()); indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
else } 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 ()); 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; IndicesPtr indices_ptr;
@ -176,7 +186,7 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
impl_.setInputCloud (cloud); impl_.setInputCloud (cloud);
impl_.setIndices (indices_ptr); impl_.setIndices (indices_ptr);
std::vector<PointIndices> clusters; std::vector<pcl::PointIndices> clusters;
impl_.extract (clusters); impl_.extract (clusters);
if (publish_indices_) if (publish_indices_)
@ -186,8 +196,10 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback (
if ((int)i >= max_clusters_) if ((int)i >= max_clusters_)
break; break;
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. // 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); pcl_msgs::PointIndices ros_pi;
pub_output_.publish (boost::make_shared<const PointIndices> (clusters[i])); 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 ()); 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 //PointCloud output_blob; // Convert from the templated output to the PointCloud blob
//pcl::toROSMsg (output, output_blob); //pcl::toROSMsg (output, output_blob);
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number. // 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 // Publish a Boost shared ptr const data
pub_output_.publish (output.makeShared ()); pub_output_.publish (output.makeShared ());
NODELET_DEBUG ("[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s", 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 ());
} }
} }
} }

16
pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp
View File

@ -42,8 +42,8 @@
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
using pcl_conversions::fromPCL; using pcl_conversions::moveFromPCL;
using pcl_conversions::toPCL; using pcl_conversions::moveToPCL;
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
@ -132,14 +132,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
if (!isValid (cloud) || !isValid (hull, "planar_hull")) if (!isValid (cloud) || !isValid (hull, "planar_hull"))
{ {
NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ()); NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ());
pub_output_.publish (boost::make_shared<const pcl_msgs::PointIndices> (inliers)); pub_output_.publish (inliers);
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid (indices)) if (indices && !isValid (indices))
{ {
NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ()); NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ());
pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers)); pub_output_.publish (inliers);
return; 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_)) if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_))
{ {
// Publish empty before return // Publish empty before return
pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers)); pub_output_.publish (inliers);
return; return;
} }
impl_.setInputPlanarHull (planar_hull.makeShared ()); 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) // 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 ()) { if (!cloud->points.empty ()) {
pcl::PointIndices pcl_inliers; pcl::PointIndices pcl_inliers;
toPCL(inliers, pcl_inliers); moveToPCL(inliers, pcl_inliers);
impl_.segment (pcl_inliers); impl_.segment (pcl_inliers);
fromPCL(pcl_inliers, inliers); moveFromPCL(pcl_inliers, inliers);
} }
// Enforce that the TF frame and the timestamp are copied // Enforce that the TF frame and the timestamp are copied
inliers.header = fromPCL(cloud->header); inliers.header = fromPCL(cloud->header);
pub_output_.publish (boost::make_shared<const PointIndices> (inliers)); pub_output_.publish (inliers);
NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ()); NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ());
} }

56
pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp
View File

@ -39,6 +39,10 @@
#include "pcl_ros/segmentation/sac_segmentation.h" #include "pcl_ros/segmentation/sac_segmentation.h"
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
using pcl_conversions::fromPCL;
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::SACSegmentation::onInit () 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) if (pub_indices_.getNumSubscribers () <= 0 && pub_model_.getNumSubscribers () <= 0)
return; return;
PointIndices inliers; pcl_msgs::PointIndices inliers;
ModelCoefficients model; pcl_msgs::ModelCoefficients model;
// Enforce that the TF frame and the timestamp are copied // 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 cloud is given, check if it's valid
if (!isValid (cloud)) if (!isValid (cloud))
{ {
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ()); NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
pub_indices_.publish (boost::make_shared<const PointIndices> (inliers)); pub_indices_.publish (inliers);
pub_model_.publish (boost::make_shared<const ModelCoefficients> (model)); pub_model_.publish (model);
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid (indices)) if (indices && !isValid (indices))
{ {
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ()); NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
pub_indices_.publish (boost::make_shared<const PointIndices> (inliers)); pub_indices_.publish (inliers);
pub_model_.publish (boost::make_shared<const ModelCoefficients> (model)); pub_model_.publish (model);
return; 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" " - 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.", " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (), 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 ()); indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
else else
NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", 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 // 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); 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) // 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 ()) if (!cloud->points.empty ()) {
impl_.segment (inliers, model); 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 // Probably need to transform the model of the plane here
@ -453,7 +464,7 @@ pcl_ros::SACSegmentationFromNormals::onInit ()
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void 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_); boost::mutex::scoped_lock lock (mutex_);
@ -551,7 +562,7 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback (
PointIndices inliers; PointIndices inliers;
ModelCoefficients model; ModelCoefficients model;
// Enforce that the TF frame and the timestamp are copied // 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) 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" " - 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.", " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (), 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 (),
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_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 ()); indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
else else
NODELET_DEBUG ("[%s::input_normals_indices_callback]\n" 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.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
getName ().c_str (), 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 (),
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_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); 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) // 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 ()) if (!cloud->points.empty ()) {
impl_.segment (inliers, model); 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 // Check if we have enough inliers, clear inliers + model if not
if ((int)inliers.indices.size () <= min_inliers_) if ((int)inliers.indices.size () <= min_inliers_)

6
pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp
View File

@ -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.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
getName ().c_str (), 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 (),
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_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_.setInputCloud (cloud);
impl_.setTargetCloud (cloud_target); impl_.setTargetCloud (cloud_target);
@ -120,7 +120,7 @@ pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cl
pub_output_.publish (output.makeShared ()); pub_output_.publish (output.makeShared ());
NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (), 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; typedef pcl_ros::SegmentDifferences SegmentDifferences;

6
pcl_ros/src/pcl_ros/surface/convex_hull.cpp
View File

@ -120,10 +120,10 @@ void
" - 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.", " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (), 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 ()); indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
else 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 // Reset the indices and surface pointers
IndicesPtr indices_ptr; IndicesPtr indices_ptr;
@ -140,7 +140,7 @@ void
if (output.points.size () >= 3) if (output.points.size () >= 3)
{ {
geometry_msgs::PolygonStamped poly; geometry_msgs::PolygonStamped poly;
poly.header = output.header; poly.header = fromPCL(output.header);
poly.polygon.points.resize (output.points.size ()); poly.polygon.points.resize (output.points.size ());
// Get three consecutive points (without copying) // Get three consecutive points (without copying)
pcl::Vector4fMap O = output.points[1].getVector4fMap (); pcl::Vector4fMap O = output.points[1].getVector4fMap ();

4
pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp
View File

@ -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" " - 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.", " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (), 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 ()); indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
else 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 // Reset the indices and surface pointers

7
pcl_ros/tools/pointcloud_to_pcd.cpp
View File

@ -37,11 +37,16 @@
// ROS core // ROS core
#include <ros/ros.h> #include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
// PCL includes // PCL includes
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <pcl/io/pcd_io.h> #include <pcl/io/pcd_io.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
using namespace std; using namespace std;
/** /**
@ -67,7 +72,7 @@ class PointCloudToPCD
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Callback // Callback
void void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud) cloud_cb (const pcl::PCLPointCloud2::ConstPtr& cloud)
{ {
if ((cloud->width * cloud->height) == 0) if ((cloud->width * cloud->height) == 0)
return; return;