36 #define BOOST_PARAMETER_MAX_ARITY 7 38 #if ( PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION == 7) 39 #include <pcl/keypoints/uniform_sampling.h> 40 #elif ( PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 8) 41 #include <pcl/filters/uniform_sampling.h> 49 DiagnosticNodelet::onInit();
50 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
51 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
53 srv_->setCallback (f);
55 pub_ = advertise<PCLIndicesMsg>(*
pnh_,
"output", 1);
77 const sensor_msgs::PointCloud2::ConstPtr&
msg)
80 pcl::PointCloud<pcl::PointXYZ>::Ptr
81 cloud (
new pcl::PointCloud<pcl::PointXYZ>);
83 pcl::UniformSampling<pcl::PointXYZ> uniform_sampling;
84 uniform_sampling.setInputCloud(cloud);
86 #if ( PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION == 7) 87 pcl::PointCloud<int> indices;
88 uniform_sampling.compute(indices);
89 #elif ( PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 8) 90 pcl::PointCloud<pcl::PointXYZ>
output;
91 uniform_sampling.filter(output);
92 pcl::PointIndicesPtr indices_ptr;
93 std::vector<int> &indices = *uniform_sampling.getIndices();
96 #if ( PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION == 7) 97 for (
size_t i = 0; i < indices.points.size(); i++) {
98 ros_indices.indices.push_back(indices.points[i]);
100 #elif ( PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 8) 101 for (
size_t i = 0; i < indices.size(); i++) {
102 ros_indices.indices.push_back(indices[i]);
105 ros_indices.header = msg->header;
void publish(const boost::shared_ptr< M > &message) const
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
void output(int index, double value)
pcl::PointIndices PCLIndicesMsg