Convert Voxel represented by MarkerArray to PointCloud

jsk_pcl_ros_utils/CMakeLists.txt

@@ -270,6 +270,8 @@ jsk_pcl_util_nodelet(src/pointcloud_relative_from_pose_stamped_nodelet.cpp
   "jsk_pcl_utils/PointCloudRelativeFromPoseStamped" "pointcloud_relative_from_pose_stamped")
 jsk_pcl_util_nodelet(src/pointcloud_to_pcd_nodelet.cpp
   "jsk_pcl_utils/PointCloudToPCD" "pointcloud_to_pcd")
+jsk_pcl_util_nodelet(src/marker_array_voxel_to_pointcloud_nodelet.cpp
+  "jsk_pcl_utils/MarkerArrayVoxelToPointCloud" "marker_array_voxel_to_pointcloud")
 add_library(jsk_pcl_ros_utils SHARED ${jsk_pcl_util_nodelet_sources})
 add_dependencies(jsk_pcl_ros_utils ${PROJECT_NAME}_gencpp ${PROJECT_NAME}_gencfg)
 target_link_libraries(jsk_pcl_ros_utils

jsk_pcl_ros_utils/include/jsk_pcl_ros_utils/marker_array_voxel_to_pointcloud.h

@@ -0,0 +1,63 @@
+// -*- mode: c++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, 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_UTILS_MARKER_ARRAY_VOXEL_TO_POINTCLOUD_H_
+#define JSK_PCL_ROS_UTILS_MARKER_ARRAY_VOXEL_TO_POINTCLOUD_H_
+
+#include <jsk_topic_tools/diagnostic_nodelet.h>
+#include <visualization_msgs/MarkerArray.h>
+
+namespace jsk_pcl_ros_utils
+{
+
+class MarkerArrayVoxelToPointCloud: public jsk_topic_tools::DiagnosticNodelet
+{
+public:
+  MarkerArrayVoxelToPointCloud(): DiagnosticNodelet("MarkerArrayVoxelToPointCloud") { }
+protected:
+  virtual void onInit();
+  virtual void subscribe();
+  virtual void unsubscribe();
+  virtual void convert(const visualization_msgs::MarkerArray::ConstPtr& marker_array_msg);
+
+  ros::Subscriber sub_;
+  ros::Publisher pub_;
+private:
+};
+
+}  // namespace jsk_pcl_ros_utils
+
+#endif  // JSK_PCL_ROS_UTILS_MARKER_ARRAY_VOXEL_TO_POINTCLOUD_H_

jsk_pcl_ros_utils/jsk_pcl_nodelets.xml

@@ -212,4 +212,10 @@
     <description>
     </description>
   </class>
+  <class name="jsk_pcl_utils/MarkerArrayVoxelToPointCloud" type="jsk_pcl_ros_utils::MarkerArrayVoxelToPointCloud"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      Convert voxel represented by visualization_msgs::MarkerArray to PointCloud.
+    </description>
+  </class>
 </library>

jsk_pcl_ros_utils/src/marker_array_voxel_to_pointcloud_nodelet.cpp

@@ -0,0 +1,91 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, 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.
+ *********************************************************************/
+
+#define BOOST_PARAMETER_MAX_ARITY 7
+
+#include <pcl_conversions/pcl_conversions.h>
+#include "jsk_pcl_ros_utils/marker_array_voxel_to_pointcloud.h"
+
+namespace jsk_pcl_ros_utils
+{
+
+  void MarkerArrayVoxelToPointCloud::onInit()
+  {
+    DiagnosticNodelet::onInit();
+
+    pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
+    onInitPostProcess();
+  }
+
+  void MarkerArrayVoxelToPointCloud::subscribe()
+  {
+    sub_ = pnh_->subscribe("input", 1, &MarkerArrayVoxelToPointCloud::convert, this);
+  }
+
+  void MarkerArrayVoxelToPointCloud::unsubscribe()
+  {
+    sub_.shutdown();
+  }
+
+  void MarkerArrayVoxelToPointCloud::convert(
+    const visualization_msgs::MarkerArray::ConstPtr& marker_array_msg)
+  {
+    vital_checker_->poke();
+
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+    cloud->is_dense = true;
+    for (size_t i=0; i<marker_array_msg->markers.size(); i++) {
+      visualization_msgs::Marker marker = marker_array_msg->markers[i];
+      for (size_t j=0; j<marker.points.size(); j++) {
+        pcl::PointXYZRGB pt;
+        pt.x = marker.points[j].x;
+        pt.y = marker.points[j].y;
+        pt.z = marker.points[j].z;
+        pt.r = marker.color.r;
+        pt.g = marker.color.g;
+        pt.b = marker.color.b;
+        cloud->points.push_back(pt);
+      }
+    }
+
+    sensor_msgs::PointCloud2 ros_cloud;
+    pcl::toROSMsg(*cloud, ros_cloud);
+    ros_cloud.header = marker_array_msg->markers[0].header;
+    pub_.publish(ros_cloud);
+  }
+
+}  // namespace jsk_pcl_ros_utils
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::MarkerArrayVoxelToPointCloud, nodelet::Nodelet);