selected_cluster_publisher_nodelet.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Ryohei Ueda and JSK Lab
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/o2r other materials provided
16  * with the distribution.
17  * * Neither the name of the JSK Lab nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
37 #include <pcl/filters/extract_indices.h>
38 
40 
41 namespace jsk_pcl_ros
42 {
44  {
45  ConnectionBasedNodelet::onInit();
46  pnh_->param("keep_organized", keep_organized_, false);
47  pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
49  }
50 
52  {
53  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(300); // 100 is enough?
54  sub_input_.subscribe(*pnh_, "input", 1);
55  sub_indices_.subscribe(*pnh_, "indices", 1);
56  sub_index_.subscribe(*pnh_, "selected_index", 1);
57  sync_->connectInput(sub_input_, sub_indices_, sub_index_);
58  sync_->registerCallback(boost::bind(&SelectedClusterPublisher::extract, this, _1, _2, _3));
59  }
60 
62  {
66  }
67 
68  void SelectedClusterPublisher::extract(const sensor_msgs::PointCloud2::ConstPtr& input,
69  const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices,
70  const jsk_recognition_msgs::Int32Stamped::ConstPtr& index)
71  {
72  if (indices->cluster_indices.size() <= index->data) {
73  NODELET_ERROR("the selected index %d is out of clusters array %lu",
74  index->data,
75  indices->cluster_indices.size());
76  return;
77  }
78  pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZRGB>());
79  pcl::fromROSMsg(*input, *input_cloud);
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>());
86  if(keep_organized_){
87  extract.setKeepOrganized(true);
88  }
89  extract.filter(*extracted_cloud);
90  sensor_msgs::PointCloud2 ros_msg;
91  pcl::toROSMsg(*extracted_cloud, ros_msg);
92  ros_msg.header = input->header;
93  pub_.publish(ros_msg);
94  }
95 }
96 
#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_
boost::shared_ptr< ros::NodeHandle > pnh_
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)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47