38 #include "jsk_recognition_msgs/ClusterPointIndices.h" 39 #include <pcl/segmentation/impl/region_growing.hpp> 49 ConnectionBasedNodelet::onInit();
50 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
51 dynamic_reconfigure::Server<Config>::CallbackType
f =
53 srv_->setCallback (f);
54 pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*
pnh_,
"output", 1);
93 pcl::PointCloud<pcl::PointNormal>
cloud;
96 pcl::RegionGrowing<pcl::PointNormal, pcl::PointNormal>
reg;
99 reg.setSearchMethod (tree);
101 reg.setInputCloud (cloud.makeShared());
103 reg.setInputNormals (cloud.makeShared());
107 std::vector <pcl::PointIndices> clusters;
108 reg.extract (clusters);
110 jsk_recognition_msgs::ClusterPointIndices
output;
111 output.header = msg->header;
113 for (
size_t i = 0; i < clusters.size(); i++) {
115 indices.header = msg->header;
116 indices.indices = clusters[i].indices;
117 output.cluster_indices.push_back(indices);
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::RegionGrowingSegmentation, nodelet::Nodelet)
virtual void configCallback(Config &config, uint32_t level)
void publish(const boost::shared_ptr< M > &message) const
jsk_pcl_ros::RegionGrowingSegmentationConfig Config
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void unsubscribe()
void output(int index, double value)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void segment(const sensor_msgs::PointCloud2::ConstPtr &msg)
double curvature_threshold_
double smoothness_threshold_
pcl::PointIndices PCLIndicesMsg