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 // PCL includes
#include <pcl/features/feature.h> #include <pcl/features/feature.h>
#include <pcl/PointIndices.h> #include <pcl_msgs/PointIndices.h>
#include "pcl_ros/pcl_nodelet.h" #include "pcl_ros/pcl_nodelet.h"
#include <message_filters/pass_through.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) input_callback (const PointCloudConstPtr &input)
{ {
PointIndices cloud; PointIndices cloud;
cloud.header.stamp = input->header.stamp; cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp;
nf_.add (boost::make_shared<PointIndices> (cloud)); 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_) 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_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
} }
else 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_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2)); sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
} }
} }
else else
// Subscribe in an old fashion to input only (no filters) // 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 ()); 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_ros/segmentation/extract_polygonal_prism_data.h"
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
using pcl_conversions::fromPCL;
using pcl_conversions::toPCL;
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::ExtractPolygonalPrismData::onInit () pcl_ros::ExtractPolygonalPrismData::onInit ()
@ -120,14 +125,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
return; return;
// Copy the header (stamp + frame_id) // Copy the header (stamp + frame_id)
pcl::PointIndices inliers; pcl_msgs::PointIndices inliers;
inliers.header = cloud->header; inliers.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) || !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::PointIndices> (inliers)); pub_output_.publish (boost::make_shared<const pcl_msgs::PointIndices> (inliers));
return; return;
} }
// If indices are given, check if they are valid // 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" " - 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 (),
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 (), 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 ()); 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_hull_callback]\n" 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.\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 (),
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 ()); 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) if (cloud->header.frame_id != hull->header.frame_id)
@ -180,10 +185,14 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_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); 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 // 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)); pub_output_.publish (boost::make_shared<const PointIndices> (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 ());
} }