37 #include <pcl/point_types.h> 38 #include <pcl/filters/passthrough.h> 39 #include <pcl/filters/voxel_grid.h> 40 #include <pcl/io/io.h> 45 #include <boost/algorithm/string.hpp> 46 #include <boost/foreach.hpp> 47 #include <boost/lexical_cast.hpp> 54 return input->slice_index;
58 return input->sequence_id;
62 return input->point_cloud.header.frame_id;
84 for (
int i = 0; i < extend; i++) {
86 pc_buffer_.push_back(jsk_recognition_msgs::SlicedPointCloudConstPtr());
93 if (pc_buffer_.size() == 1) {
119 pcl::PointCloud<pcl::PointXYZRGB>::Ptr concatenated_cloud (
new pcl::PointCloud<pcl::PointXYZRGB>);
120 for (
size_t i = 0; i <
pc_buffer_.size(); i++)
126 pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (
new pcl::PointCloud<pcl::PointXYZRGB>);
127 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_tmp_cloud (
new pcl::PointCloud<pcl::PointXYZRGB>);
128 fromROSMsg(
pc_buffer_[i]->point_cloud, *tmp_cloud);
129 if (tmp_cloud->points.size() == 0) {
139 *transformed_tmp_cloud,
142 for (
size_t j = 0; j < transformed_tmp_cloud->points.size(); j++) {
143 concatenated_cloud->points.push_back(transformed_tmp_cloud->points[j]);
146 sensor_msgs::PointCloud2 out;
147 toROSMsg(*concatenated_cloud, out);
148 out.header =
pc_buffer_[0]->point_cloud.header;
155 ConnectionBasedNodelet::onInit();
158 pub_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
#define NODELET_INFO_STREAM(...)
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
int getPointcloudSequenceID(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input)
int getPointcloudID(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input)
std::string getPointcloudFrameId(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input)
std::vector< jsk_recognition_msgs::SlicedPointCloudConstPtr > pc_buffer_
void pointCB(const jsk_recognition_msgs::SlicedPointCloudConstPtr &input)
tf::TransformListener tf_listener
#define NODELET_INFO(...)
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
virtual void unsubscribe()
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::VoxelGridDownsampleDecoder, nodelet::Nodelet)