octree_voxel_grid_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #define BOOST_PARAMETER_MAX_ARITY 7
38 #include <std_msgs/Float32.h>
40 #include <pcl/common/common.h>
42 
43 namespace jsk_pcl_ros
44 {
45  template <class PointT>
46  void OctreeVoxelGrid::generateVoxelCloudImpl(const sensor_msgs::PointCloud2ConstPtr& input_msg)
47  {
48 
49  typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
50  typename pcl::PointCloud<PointT>::Ptr cloud_voxeled (new pcl::PointCloud<PointT> ());
51  pcl::fromROSMsg(*input_msg, *cloud);
52 
53  // generate octree
54  typename pcl::octree::OctreePointCloud<PointT> octree(resolution_);
55  // add point cloud to octree
56  octree.setInputCloud(cloud);
57  octree.addPointsFromInputCloud();
58  // get points where grid is occupied
59  typename pcl::octree::OctreePointCloud<PointT>::AlignedPointTVector point_vec;
60  octree.getOccupiedVoxelCenters(point_vec);
61  // put points into point cloud
62  cloud_voxeled->width = point_vec.size();
63  cloud_voxeled->height = 1;
64  for (int i = 0; i < point_vec.size(); i++) {
65  cloud_voxeled->push_back(point_vec[i]);
66  }
67 
68  // publish point cloud
69  sensor_msgs::PointCloud2 output_msg;
70  toROSMsg(*cloud_voxeled, output_msg);
71  output_msg.header = input_msg->header;
72  pub_cloud_.publish(output_msg);
73 
74  // publish marker
76  visualization_msgs::Marker marker_msg;
77  marker_msg.type = visualization_msgs::Marker::CUBE_LIST;
78  marker_msg.scale.x = resolution_;
79  marker_msg.scale.y = resolution_;
80  marker_msg.scale.z = resolution_;
81  marker_msg.header = input_msg->header;
82  marker_msg.pose.orientation.w = 1.0;
83  if (marker_color_ == "flat") {
84  marker_msg.color = jsk_topic_tools::colorCategory20(0);
85  marker_msg.color.a = marker_color_alpha_;
86  }
87 
88  // compute min and max
89  Eigen::Vector4f minpt, maxpt;
90  pcl::getMinMax3D<PointT>(*cloud_voxeled, minpt, maxpt);
91  PointT p;
92  for (size_t i = 0; i < cloud_voxeled->size(); i++) {
93  p = cloud_voxeled->at(i);
94  geometry_msgs::Point point_ros;
95  point_ros.x = p.x;
96  point_ros.y = p.y;
97  point_ros.z = p.z;
98  marker_msg.points.push_back(point_ros);
99  if (marker_color_ == "flat") {
100  marker_msg.colors.push_back(jsk_topic_tools::colorCategory20(0));
101  }
102  else if (marker_color_ == "z") {
103  marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.z - minpt[2]) / (maxpt[2] - minpt[2])));
104  }
105  else if (marker_color_ == "x") {
106  marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.x - minpt[0]) / (maxpt[0] - minpt[0])));
107  }
108  else if (marker_color_ == "y") {
109  marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.y - minpt[1]) / (maxpt[1] - minpt[1])));
110  }
111  marker_msg.colors[marker_msg.colors.size() - 1].a = marker_color_alpha_;
112  }
113  pub_marker_.publish(marker_msg);
114 
115  // publish marker_array also for convenience
116  visualization_msgs::MarkerArray marker_array_msg;
117  marker_array_msg.markers.push_back(marker_msg);
118  pub_marker_array_.publish(marker_array_msg);
119  }
120  }
121 
122  void OctreeVoxelGrid::generateVoxelCloud(const sensor_msgs::PointCloud2ConstPtr& input_msg)
123  {
124  boost::mutex::scoped_lock lock(mutex_);
125  if (resolution_ == 0.0) {
126  pub_cloud_.publish(input_msg);
127 
128  }
129  else {
130  if (jsk_recognition_utils::hasField("rgb", *input_msg) &&
131  jsk_recognition_utils::hasField("normal_x", *input_msg) &&
132  jsk_recognition_utils::hasField("normal_y", *input_msg) &&
133  jsk_recognition_utils::hasField("normal_z", *input_msg)) {
134  generateVoxelCloudImpl<pcl::PointXYZRGBNormal>(input_msg);
135  }
136  else if (jsk_recognition_utils::hasField("rgb", *input_msg)) {
137  generateVoxelCloudImpl<pcl::PointXYZRGB>(input_msg);
138  }
139  else if (jsk_recognition_utils::hasField("normal_x", *input_msg) &&
140  jsk_recognition_utils::hasField("normal_y", *input_msg) &&
141  jsk_recognition_utils::hasField("normal_z", *input_msg)) {
142  generateVoxelCloudImpl<pcl::PointNormal>(input_msg);
143  }
144  else {
145  generateVoxelCloudImpl<pcl::PointXYZ>(input_msg);
146  }
147  }
148  std_msgs::Float32 resolution;
149  resolution.data = resolution_;
150  pub_octree_resolution_.publish(resolution);
151  }
152 
154  {
155  sub_input_ = pnh_->subscribe("input", 1, &OctreeVoxelGrid::generateVoxelCloud, this);
156  }
157 
159  {
161  }
162 
163  void OctreeVoxelGrid::configCallback(Config &config, uint32_t level)
164  {
165  boost::mutex::scoped_lock lock(mutex_);
166  resolution_ = config.resolution;
167  publish_marker_flag_ = config.publish_marker;
168  marker_color_ = config.marker_color;
169  marker_color_alpha_ = config.marker_color_alpha;
170  }
171 
173  {
174  DiagnosticNodelet::onInit();
175  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
176  dynamic_reconfigure::Server<Config>::CallbackType f =
177  boost::bind (&OctreeVoxelGrid::configCallback, this, _1, _2);
178  srv_->setCallback (f);
179 
180  pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
181  pub_marker_ = advertise<visualization_msgs::Marker>(*pnh_, "output_marker", 1);
182  pub_marker_array_ = advertise<visualization_msgs::MarkerArray>(*pnh_, "output_marker_array", 1);
183  pub_octree_resolution_ = advertise<std_msgs::Float32>(*pnh_, "output_resolution", 1);
184 
186  }
187 }
188 
resolution
std_msgs::ColorRGBA heatColor(double v)
void publish(const boost::shared_ptr< M > &message) const
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
bool hasField(const std::string &field_name, const sensor_msgs::PointCloud2 &msg)
virtual void configCallback(Config &config, uint32_t level)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
boost::shared_ptr< ros::NodeHandle > pnh_
jsk_pcl_ros::OctreeVoxelGridConfig Config
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::OctreeVoxelGrid, nodelet::Nodelet)
pcl::PointXYZRGB PointT
std_msgs::ColorRGBA colorCategory20(int i)
p
cloud
void generateVoxelCloudImpl(const sensor_msgs::PointCloud2ConstPtr &input)
virtual void generateVoxelCloud(const sensor_msgs::PointCloud2ConstPtr &input)


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