37 #include <pcl/filters/extract_indices.h> 45 ConnectionBasedNodelet::onInit();
47 pub_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
53 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(300);
69 const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices,
70 const jsk_recognition_msgs::Int32Stamped::ConstPtr& index)
72 if (indices->cluster_indices.size() <= index->data) {
73 NODELET_ERROR(
"the selected index %d is out of clusters array %lu",
75 indices->cluster_indices.size());
78 pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud (
new pcl::PointCloud<pcl::PointXYZRGB>());
80 pcl::ExtractIndices<pcl::PointXYZRGB>
extract;
81 pcl::PointIndices::Ptr pcl_indices (
new pcl::PointIndices);
82 pcl_indices->indices = indices->cluster_indices[index->data].indices;
83 extract.setInputCloud(input_cloud);
84 extract.setIndices(pcl_indices);
85 pcl::PointCloud<pcl::PointXYZRGB>::Ptr extracted_cloud(
new pcl::PointCloud<pcl::PointXYZRGB>());
87 extract.setKeepOrganized(
true);
89 extract.filter(*extracted_cloud);
90 sensor_msgs::PointCloud2 ros_msg;
92 ros_msg.header = input->header;
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
message_filters::Subscriber< jsk_recognition_msgs::ClusterPointIndices > sub_indices_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::SelectedClusterPublisher, nodelet::Nodelet)
message_filters::Subscriber< jsk_recognition_msgs::Int32Stamped > sub_index_
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
virtual void extract(const sensor_msgs::PointCloud2::ConstPtr &input, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &indices, const jsk_recognition_msgs::Int32Stamped::ConstPtr &index)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
virtual void unsubscribe()