pointcloud2_to_map.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014-2017, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <ros/ros.h>
31 
35 #include <nav_msgs/OccupancyGrid.h>
36 #include <sensor_msgs/PointCloud2.h>
37 
38 #include <string>
39 #include <vector>
40 
43 
45 {
46 private:
52 
53  nav_msgs::OccupancyGrid map_;
58 
59  double z_min_, z_max_;
60  std::string global_frame_;
61  std::string robot_frame_;
62 
63  unsigned int width_;
64  unsigned int height_;
65  float origin_x_;
66  float origin_y_;
67 
68  std::vector<costmap_cspace::PointcloudAccumurator<sensor_msgs::PointCloud2>> accums_;
69 
70 public:
72  : nh_()
73  , pnh_("~")
74  , tfl_(tfbuf_)
75  , accums_(2)
76  {
78  pnh_.param("z_min", z_min_, 0.1);
79  pnh_.param("z_max", z_max_, 1.0);
80  pnh_.param("global_frame", global_frame_, std::string("map"));
81  pnh_.param("robot_frame", robot_frame_, std::string("base_link"));
82 
83  double accum_duration;
84  pnh_.param("accum_duration", accum_duration, 1.0);
85  accums_[0].reset(ros::Duration(accum_duration));
86  accums_[1].reset(ros::Duration(0.0));
87 
88  pub_map_ = neonavigation_common::compat::advertise<nav_msgs::OccupancyGrid>(
89  nh_, "map_local",
90  pnh_, "map", 1, true);
91  sub_cloud_ = nh_.subscribe<sensor_msgs::PointCloud2>(
92  "cloud", 100,
93  boost::bind(&Pointcloud2ToMapNode::cbCloud, this, _1, false));
94  sub_cloud_single_ = nh_.subscribe<sensor_msgs::PointCloud2>(
95  "cloud_singleshot", 100,
96  boost::bind(&Pointcloud2ToMapNode::cbCloud, this, _1, true));
97 
98  int width_param;
99  pnh_.param("width", width_param, 30);
100  height_ = width_ = width_param;
101  map_.header.frame_id = global_frame_;
102 
103  double resolution;
104  pnh_.param("resolution", resolution, 0.1);
105  map_.info.resolution = resolution;
106  map_.info.width = width_;
107  map_.info.height = height_;
108  map_.data.resize(map_.info.width * map_.info.height);
109 
110  double hz;
111  pnh_.param("hz", hz, 1.0);
112  publish_interval_ = ros::Duration(1.0 / hz);
113  }
114 
115 private:
116  void cbCloud(const sensor_msgs::PointCloud2::ConstPtr& cloud, const bool singleshot)
117  {
118  sensor_msgs::PointCloud2 cloud_global;
119  geometry_msgs::TransformStamped trans;
120  try
121  {
122  trans = tfbuf_.lookupTransform(global_frame_, cloud->header.frame_id,
123  cloud->header.stamp, ros::Duration(0.5));
124  }
125  catch (tf2::TransformException& e)
126  {
127  ROS_WARN("%s", e.what());
128  return;
129  }
130  tf2::doTransform(*cloud, cloud_global, trans);
131 
132  const int buffer = singleshot ? 1 : 0;
134  cloud_global, cloud_global.header.stamp));
135 
136  ros::Time now = cloud->header.stamp;
137  if (published_ + publish_interval_ > now)
138  return;
139  published_ = now;
140 
141  float robot_z;
142  try
143  {
145  tf2::fromMsg(tfbuf_.lookupTransform(global_frame_, robot_frame_, ros::Time(0)), trans);
146 
147  auto pos = trans.getOrigin();
148  float x = static_cast<int>(pos.x() / map_.info.resolution) * map_.info.resolution;
149  float y = static_cast<int>(pos.y() / map_.info.resolution) * map_.info.resolution;
150  map_.info.origin.position.x = x - map_.info.width * map_.info.resolution * 0.5;
151  map_.info.origin.position.y = y - map_.info.height * map_.info.resolution * 0.5;
152  map_.info.origin.position.z = 0.0;
153  map_.info.origin.orientation.w = 1.0;
154  origin_x_ = x - width_ * map_.info.resolution * 0.5;
155  origin_y_ = y - height_ * map_.info.resolution * 0.5;
156  robot_z = pos.z();
157  }
158  catch (tf2::TransformException& e)
159  {
160  ROS_WARN("%s", e.what());
161  return;
162  }
163  for (auto& cell : map_.data)
164  cell = 0;
165 
166  for (auto& accum : accums_)
167  {
168  for (auto& pc : accum)
169  {
173  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
174  {
175  if (*iter_z - robot_z < z_min_ || z_max_ < *iter_z - robot_z)
176  continue;
177  unsigned int x = int(
178  (*iter_x - map_.info.origin.position.x) / map_.info.resolution);
179  unsigned int y = int(
180  (*iter_y - map_.info.origin.position.y) / map_.info.resolution);
181  if (x >= map_.info.width || y >= map_.info.height)
182  continue;
183  map_.data[x + y * map_.info.width] = 100;
184  }
185  }
186  }
187 
188  pub_map_.publish(map_);
189  }
190 };
191 
192 int main(int argc, char** argv)
193 {
194  ros::init(argc, argv, "pointcloud2_to_map");
195 
197  ros::spin();
198 
199  return 0;
200 }
std::vector< costmap_cspace::PointcloudAccumurator< sensor_msgs::PointCloud2 > > accums_
void publish(const boost::shared_ptr< M > &message) const
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
nav_msgs::OccupancyGrid map_
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
ROSCPP_DECL void spin(Spinner &spinner)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void fromMsg(const A &, B &b)
ros::Subscriber sub_cloud_single_
void cbCloud(const sensor_msgs::PointCloud2::ConstPtr &cloud, const bool singleshot)
TFSIMD_FORCE_INLINE const tfScalar & x() const
ros::Duration publish_interval_
int main(int argc, char **argv)
ros::Subscriber sub_cloud_
tf2_ros::TransformListener tfl_


costmap_cspace
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:29