[jsk_pcl_ros] PointIndicesToPointCloud as a simple cli

See: jsk-ros-pkg#1187 (comment)

jsk_pcl_ros/CMakeLists.txt

@@ -326,6 +326,8 @@ jsk_pcl_nodelet(src/roi_clipper_nodelet.cpp
   "jsk_pcl/ROIClipper" "roi_clipper")
 jsk_pcl_nodelet(src/point_indices_to_mask_image_nodelet.cpp
   "jsk_pcl/PointIndicesToMaskImage" "point_indices_to_mask_image")
+jsk_pcl_nodelet(src/point_indices_to_pointcloud_nodelet.cpp
+  "jsk_pcl/PointIndicesToPointCloud" "point_indices_to_pointcloud")
 jsk_pcl_nodelet(src/mask_image_to_depth_considered_mask_image_nodelet.cpp
   "jsk_pcl/MaskImageToDepthConsideredMaskImage" "mask_image_to_depth_considered_mask_image")
 jsk_pcl_nodelet(src/mask_image_to_point_indices_nodelet.cpp

jsk_pcl_ros/include/jsk_pcl_ros/point_indices_to_pointcloud.h

@@ -0,0 +1,81 @@
+// -*- mode: c++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, JSK Lab
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/o2r other materials provided
+ *     with the distribution.
+ *   * Neither the name of the JSK Lab nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#ifndef JSK_PCL_ROS_POINTCLOUD_TO_CLUSTER_POINT_INDICES_H_
+#define JSK_PCL_ROS_POINTCLOUD_TO_CLUSTER_POINT_INDICES_H_
+
+#include "jsk_topic_tools/diagnostic_nodelet.h"
+#include "jsk_pcl_ros/pcl_conversion_util.h"
+
+#include <sensor_msgs/PointCloud2.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+
+namespace jsk_pcl_ros
+{
+  class PointIndicesToPointCloud: public jsk_topic_tools::DiagnosticNodelet
+  {
+  public:
+    typedef pcl::PointXYZRGB PointT;
+    typedef message_filters::sync_policies::ExactTime<
+    PCLIndicesMsg,
+    sensor_msgs::PointCloud2 > SyncPolicy;
+    typedef message_filters::sync_policies::ApproximateTime<
+    PCLIndicesMsg,
+    sensor_msgs::PointCloud2 > ApproximateSyncPolicy;
+
+    PointIndicesToPointCloud(): DiagnosticNodelet("PointIndicesToPointCloud") {}
+  protected:
+    virtual void onInit();
+    virtual void subscribe();
+    virtual void unsubscribe();
+    virtual void convert(
+      const PCLIndicesMsg::ConstPtr& indices_msg,
+      const sensor_msgs::PointCloud2::ConstPtr& msg);
+
+    bool approximate_sync_;
+    boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> >sync_;
+    boost::shared_ptr<message_filters::Synchronizer<ApproximateSyncPolicy> >async_;
+    ros::Publisher pub_;
+    message_filters::Subscriber<sensor_msgs::PointCloud2> sub_cloud_;
+    message_filters::Subscriber<PCLIndicesMsg> sub_indices_;
+  private:
+  };
+
+}
+
+#endif

jsk_pcl_ros/jsk_pcl_nodelets.xml

@@ -187,6 +187,11 @@
          base_class_type="nodelet::Nodelet">
     Generate mask image out of point indices
   </class>
+  <class name="jsk_pcl/PointIndicesToPointCloud"
+         type="jsk_pcl_ros::PointIndicesToPointCloud"
+         base_class_type="nodelet::Nodelet">
+    Extract point cloud with point indices
+  </class>
   <class name="jsk_pcl/MaskImageToDepthConsideredMaskImage"
          type="jsk_pcl_ros::MaskImageToDepthConsideredMaskImage"
          base_class_type="nodelet::Nodelet">

jsk_pcl_ros/src/point_indices_to_pointcloud_nodelet.cpp

@@ -0,0 +1,98 @@
+// -*- mode: c++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, JSK Lab
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/o2r other materials provided
+ *     with the distribution.
+ *   * Neither the name of the JSK Lab nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "jsk_pcl_ros/point_indices_to_pointcloud.h"
+#include "jsk_pcl_ros/pcl_conversion_util.h"
+
+#include <pcl/filters/extract_indices.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/PointCloud2.h>
+
+namespace jsk_pcl_ros
+{
+  void PointIndicesToPointCloud::onInit()
+  {
+    DiagnosticNodelet::onInit();
+    pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
+  }
+
+  void PointIndicesToPointCloud::subscribe()
+  {
+    sub_indices_.subscribe(*pnh_, "input", 1);
+    sub_cloud_.subscribe(*pnh_, "input/cloud", 1);
+    if (approximate_sync_) {
+      async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(100);
+      async_->connectInput(sub_indices_, sub_cloud_);
+      async_->registerCallback(
+        boost::bind(&PointIndicesToPointCloud::convert, this, _1, _2));
+    }
+    else {
+      sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
+      sync_->connectInput(sub_indices_, sub_cloud_);
+      sync_->registerCallback(
+        boost::bind(&PointIndicesToPointCloud::convert, this, _1, _2));
+    }
+  }
+
+  void PointIndicesToPointCloud::unsubscribe()
+  {
+    sub_cloud_.unsubscribe();
+    sub_indices_.unsubscribe();
+  }
+
+  void PointIndicesToPointCloud::convert(
+    const PCLIndicesMsg::ConstPtr& indices_msg,
+    const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
+  {
+    pcl::PointCloud<PointT>::Ptr input(new pcl::PointCloud<PointT>());
+    pcl::fromROSMsg(*cloud_msg, *input);
+    pcl::PointIndices::Ptr indices (new pcl::PointIndices ());
+    pcl_conversions::toPCL(*indices_msg, *indices);
+
+    pcl::ExtractIndices<PointT> extract;
+    extract.setInputCloud(input);
+    extract.setIndices(indices);
+    pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>());
+    extract.filter(*output);
+
+    sensor_msgs::PointCloud2::Ptr out_cloud(new sensor_msgs::PointCloud2);
+    pcl::toROSMsg(*output, *out_cloud);
+    out_cloud->header = cloud_msg->header;
+    pub_.publish(out_cloud);
+  }
+}
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::PointIndicesToPointCloud, nodelet::Nodelet);