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)
{
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<sensor_msgs::PointCloud2> impl_;
pcl::ExtractIndices<pcl::PCLPointCloud2> impl_;
public:
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)
{
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<sensor_msgs::PointCloud2> impl_;
pcl::PassThrough<pcl::PCLPointCloud2> impl_;
public:
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,
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<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_;
/** \brief The PCL filter implementation used. */
pcl::ProjectInliers<PointCloud2> impl_;
pcl::ProjectInliers<pcl::PCLPointCloud2> impl_;
/** \brief Nodelet initialization routine. */
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,
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<sensor_msgs::PointCloud2> impl_;
pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> impl_;
public:
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)
{
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<sensor_msgs::PointCloud2> impl_;
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> impl_;
public:
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_;
/** \brief The PCL filter implementation used. */
pcl::VoxelGrid<sensor_msgs::PointCloud2> impl_;
pcl::VoxelGrid<pcl::PCLPointCloud2> impl_;
/** \brief Call the actual filter.
* \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/approximate_time.h>
#include <sensor_msgs/PointCloud2.h>
namespace pcl_ros
{
/** \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>
// PCL includes
#include <pcl_msgs/PointIndices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl_msgs/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#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;

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
* synchronizer */
message_filters::PassThrough<PointIndices> nf_pi_;
message_filters::PassThrough<pcl_msgs::PointIndices> 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<PointIndices> (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

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

@ -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<PointIn> PointCloudIn;
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)
{
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);
}
//////////////////////////////////////////////////////////////////////////////////////////////

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

@ -40,6 +40,8 @@
#include "pcl_ros/transforms.h"
#include "pcl_ros/io/concatenate_data.h"
#include <pcl_conversions/pcl_conversions.h>
//////////////////////////////////////////////////////////////////////////////////////////////
void
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_ros/io/concatenate_fields.h"
#include <pcl_conversions/pcl_conversions.h>
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
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 ());
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<PointCloud2> (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<std::string> (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<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 ());
}

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

@ -37,8 +37,15 @@
#include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h>
#include <pcl/PointIndices.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
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<PointIndices> clusters;
std::vector<pcl::PointIndices> 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<const PointIndices> (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 ());
}
}
}

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

@ -42,8 +42,8 @@
#include <pcl_conversions/pcl_conversions.h>
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<const pcl_msgs::PointIndices> (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<const pcl::PointIndices> (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<const pcl::PointIndices> (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<const PointIndices> (inliers));
pub_output_.publish (inliers);
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/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
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<const PointIndices> (inliers));
pub_model_.publish (boost::make_shared<const ModelCoefficients> (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<const PointIndices> (inliers));
pub_model_.publish (boost::make_shared<const ModelCoefficients> (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_)

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.",
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;

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"
" - 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 ();

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"
" - 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

7
pcl_ros/tools/pointcloud_to_pcd.cpp
View File

@ -37,11 +37,16 @@
// ROS core
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
// PCL includes
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
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;