octree_voxel_grid_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 #include "jsk_pcl_ros/octree_voxel_grid.h"
00038 #include <std_msgs/Float32.h>
00039 #include <jsk_topic_tools/color_utils.h>
00040 #include <pcl/common/common.h>
00041 #include <jsk_recognition_utils/pcl_ros_util.h>
00042 
00043 namespace jsk_pcl_ros
00044 {
00045   template <class PointT>
00046   void OctreeVoxelGrid::generateVoxelCloudImpl(const sensor_msgs::PointCloud2ConstPtr& input_msg)
00047   {
00048 
00049     typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
00050     typename pcl::PointCloud<PointT>::Ptr cloud_voxeled (new pcl::PointCloud<PointT> ());
00051     pcl::fromROSMsg(*input_msg, *cloud);
00052 
00053     // generate octree
00054     typename pcl::octree::OctreePointCloud<PointT> octree(resolution_);
00055     // add point cloud to octree
00056     octree.setInputCloud(cloud);
00057     octree.addPointsFromInputCloud();
00058     // get points where grid is occupied
00059     typename pcl::octree::OctreePointCloud<PointT>::AlignedPointTVector point_vec;
00060     octree.getOccupiedVoxelCenters(point_vec);
00061     // put points into point cloud
00062     cloud_voxeled->width = point_vec.size();
00063     cloud_voxeled->height = 1;
00064     for (int i = 0; i < point_vec.size(); i++) {
00065       cloud_voxeled->push_back(point_vec[i]);
00066     }
00067 
00068     // publish point cloud
00069     sensor_msgs::PointCloud2 output_msg;
00070     toROSMsg(*cloud_voxeled, output_msg);
00071     output_msg.header = input_msg->header;
00072     pub_cloud_.publish(output_msg);
00073 
00074     // publish marker
00075     if (publish_marker_flag_) {
00076       visualization_msgs::Marker marker_msg;
00077       marker_msg.type = visualization_msgs::Marker::CUBE_LIST;
00078       marker_msg.scale.x = resolution_;
00079       marker_msg.scale.y = resolution_;
00080       marker_msg.scale.z = resolution_;
00081       marker_msg.header = input_msg->header;
00082       marker_msg.pose.orientation.w = 1.0;
00083       if (marker_color_ == "flat") {
00084         marker_msg.color = jsk_topic_tools::colorCategory20(0);
00085         marker_msg.color.a = marker_color_alpha_;
00086       }
00087       
00088       // compute min and max
00089       Eigen::Vector4f minpt, maxpt;
00090       pcl::getMinMax3D<PointT>(*cloud_voxeled, minpt, maxpt);
00091       PointT p;
00092       for (size_t i = 0; i < cloud_voxeled->size(); i++) {
00093         p = cloud_voxeled->at(i);
00094         geometry_msgs::Point point_ros;
00095         point_ros.x = p.x;
00096         point_ros.y = p.y;
00097         point_ros.z = p.z;
00098         marker_msg.points.push_back(point_ros);
00099         if (marker_color_ == "flat") {
00100           marker_msg.colors.push_back(jsk_topic_tools::colorCategory20(0));
00101         }
00102         else if (marker_color_ == "z") {
00103           marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.z - minpt[2]) / (maxpt[2] - minpt[2])));
00104         }
00105         else if (marker_color_ == "x") {
00106           marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.x - minpt[0]) / (maxpt[0] - minpt[0])));
00107         }
00108         else if (marker_color_ == "y") {
00109           marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.y - minpt[1]) / (maxpt[1] - minpt[1])));
00110         }
00111         marker_msg.colors[marker_msg.colors.size() - 1].a = marker_color_alpha_;
00112       }
00113       pub_marker_.publish(marker_msg);
00114 
00115       // publish marker_array also for convenience
00116       visualization_msgs::MarkerArray marker_array_msg;
00117       marker_array_msg.markers.push_back(marker_msg);
00118       pub_marker_array_.publish(marker_array_msg);
00119     }
00120   }
00121 
00122   void OctreeVoxelGrid::generateVoxelCloud(const sensor_msgs::PointCloud2ConstPtr& input_msg)
00123   {
00124     boost::mutex::scoped_lock lock(mutex_);
00125     if (resolution_ == 0.0) {
00126       pub_cloud_.publish(input_msg);
00127       
00128     }
00129     else {
00130       if (jsk_recognition_utils::hasField("rgb", *input_msg) &&
00131           jsk_recognition_utils::hasField("normal_x", *input_msg) &&
00132           jsk_recognition_utils::hasField("normal_y", *input_msg) &&
00133           jsk_recognition_utils::hasField("normal_z", *input_msg)) {
00134         generateVoxelCloudImpl<pcl::PointXYZRGBNormal>(input_msg);
00135       }
00136       else if (jsk_recognition_utils::hasField("rgb", *input_msg)) {
00137         generateVoxelCloudImpl<pcl::PointXYZRGB>(input_msg);
00138       }
00139       else if (jsk_recognition_utils::hasField("normal_x", *input_msg) &&
00140                jsk_recognition_utils::hasField("normal_y", *input_msg) &&
00141                jsk_recognition_utils::hasField("normal_z", *input_msg)) {
00142         generateVoxelCloudImpl<pcl::PointNormal>(input_msg);
00143       }
00144       else {
00145         generateVoxelCloudImpl<pcl::PointXYZ>(input_msg);
00146       }
00147     }
00148     std_msgs::Float32 resolution;
00149     resolution.data = resolution_;
00150     pub_octree_resolution_.publish(resolution);
00151   }
00152 
00153   void OctreeVoxelGrid::subscribe()
00154   {
00155     sub_input_ = pnh_->subscribe("input", 1, &OctreeVoxelGrid::generateVoxelCloud, this);
00156   }
00157 
00158   void OctreeVoxelGrid::unsubscribe()
00159   {
00160     sub_input_.shutdown();
00161   }
00162 
00163   void OctreeVoxelGrid::configCallback(Config &config, uint32_t level)
00164   {
00165     boost::mutex::scoped_lock lock(mutex_);
00166     resolution_ = config.resolution;
00167     publish_marker_flag_ = config.publish_marker;
00168     marker_color_ = config.marker_color;
00169     marker_color_alpha_ = config.marker_color_alpha;
00170   }
00171 
00172   void OctreeVoxelGrid::onInit(void)
00173   {
00174     DiagnosticNodelet::onInit();
00175     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00176     dynamic_reconfigure::Server<Config>::CallbackType f =
00177       boost::bind (&OctreeVoxelGrid::configCallback, this, _1, _2);
00178     srv_->setCallback (f);
00179 
00180     pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00181     pub_marker_ = advertise<visualization_msgs::Marker>(*pnh_, "output_marker", 1);
00182     pub_marker_array_ = advertise<visualization_msgs::MarkerArray>(*pnh_, "output_marker_array", 1);
00183     pub_octree_resolution_ = advertise<std_msgs::Float32>(*pnh_, "output_resolution", 1);
00184 
00185     onInitPostProcess();
00186   }
00187 }
00188 
00189 #include <pluginlib/class_list_macros.h>
00190 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::OctreeVoxelGrid, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:50