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

Use the PointIndices from pcl_msgs

Browse Source
This commit is contained in:
William Woodall 2013-07-08 17:38:48 -07:00 committed by Paul Bovbel
parent 018bb008d4
commit 4e64cb25e7
4 changed files with 24 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
pcl_ros/include/pcl_ros/features/feature.h
View File

@ -40,7 +40,7 @@
// PCL includes
#include <pcl/features/feature.h>
#include <pcl/PointIndices.h>
#include <pcl_msgs/PointIndices.h>
#include "pcl_ros/pcl_nodelet.h"
#include <message_filters/pass_through.h>

2
pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.h
View File

@ -93,7 +93,7 @@ namespace pcl_ros
input_callback (const PointCloudConstPtr &input)
{
PointIndices cloud;
cloud.header.stamp = input->header.stamp;
cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
nf_.add (boost::make_shared<PointIndices> (cloud));
}

6
pcl_ros/src/pcl_ros/filters/filter.cpp
View File

@ -133,20 +133,20 @@ pcl_ros::Filter::onInit ()
if (approximate_sync_)
{
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, pcl::PointIndices> > >(max_queue_size_);
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_);
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
}
else
{
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, pcl::PointIndices> > >(max_queue_size_);
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, pcl_msgs::PointIndices> > >(max_queue_size_);
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
}
}
else
// Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl::PointIndicesConstPtr ()));
sub_input_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr ()));
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ());
}

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

@ -40,6 +40,11 @@
#include "pcl_ros/segmentation/extract_polygonal_prism_data.h"
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
using pcl_conversions::fromPCL;
using pcl_conversions::toPCL;
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::ExtractPolygonalPrismData::onInit ()
@ -120,14 +125,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
return;
// Copy the header (stamp + frame_id)
pcl::PointIndices inliers;
inliers.header = cloud->header;
pcl_msgs::PointIndices inliers;
inliers.header = fromPCL(cloud->header);
// If cloud is given, check if it's valid
if (!isValid (cloud) || !isValid (hull, "planar_hull"))
{
NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ());
pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
pub_output_.publish (boost::make_shared<const pcl_msgs::PointIndices> (inliers));
return;
}
// If indices are given, check if they are valid
@ -145,16 +150,16 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ());
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ());
///
if (cloud->header.frame_id != hull->header.frame_id)
@ -180,10 +185,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
impl_.setIndices (indices_ptr);
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
if (!cloud->points.empty ())
impl_.segment (inliers);
if (!cloud->points.empty ()) {
pcl::PointIndices pcl_inliers;
toPCL(inliers, pcl_inliers);
impl_.segment (pcl_inliers);
fromPCL(pcl_inliers, inliers);
}
// Enforce that the TF frame and the timestamp are copied
inliers.header = cloud->header;
inliers.header = fromPCL(cloud->header);
pub_output_.publish (boost::make_shared<const PointIndices> (inliers));
NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ());
}