voxel_layer.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, 2013, Willow Garage, Inc.
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/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. 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  * Author: Eitan Marder-Eppstein
36  * David V. Lu!!
37  *********************************************************************/
38 #include <costmap_2d/voxel_layer.h>
41 
42 #define VOXEL_BITS 16
44 
47 using costmap_2d::FREE_SPACE;
48 
50 using costmap_2d::Observation;
51 
52 namespace costmap_2d
53 {
54 
55 void VoxelLayer::onInitialize()
56 {
57  ObstacleLayer::onInitialize();
58  ros::NodeHandle private_nh("~/" + name_);
59 
60  private_nh.param("publish_voxel_map", publish_voxel_, false);
61  if (publish_voxel_)
62  voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);
63 
64  clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
65 }
66 
67 void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
68 {
69  voxel_dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh);
70  dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb = boost::bind(
71  &VoxelLayer::reconfigureCB, this, _1, _2);
72  voxel_dsrv_->setCallback(cb);
73 }
74 
75 VoxelLayer::~VoxelLayer()
76 {
77  if (voxel_dsrv_)
78  delete voxel_dsrv_;
79 }
80 
81 void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
82 {
83  enabled_ = config.enabled;
84  footprint_clearing_enabled_ = config.footprint_clearing_enabled;
85  max_obstacle_height_ = config.max_obstacle_height;
86  size_z_ = config.z_voxels;
87  origin_z_ = config.origin_z;
88  z_resolution_ = config.z_resolution;
89  unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);
90  mark_threshold_ = config.mark_threshold;
91  combination_method_ = config.combination_method;
92  matchSize();
93 }
94 
95 void VoxelLayer::matchSize()
96 {
97  ObstacleLayer::matchSize();
98  voxel_grid_.resize(size_x_, size_y_, size_z_);
99  ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
100 }
101 
102 void VoxelLayer::reset()
103 {
104  deactivate();
105  resetMaps();
106  voxel_grid_.reset();
107  activate();
108 }
109 
110 void VoxelLayer::resetMaps()
111 {
112  Costmap2D::resetMaps();
113  voxel_grid_.reset();
114 }
115 
116 void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
117  double* min_y, double* max_x, double* max_y)
118 {
119  if (rolling_window_)
120  updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
121  if (!enabled_)
122  return;
123  useExtraBounds(min_x, min_y, max_x, max_y);
124 
125  bool current = true;
126  std::vector<Observation> observations, clearing_observations;
127 
128  // get the marking observations
129  current = getMarkingObservations(observations) && current;
130 
131  // get the clearing observations
132  current = getClearingObservations(clearing_observations) && current;
133 
134  // update the global current status
135  current_ = current;
136 
137  // raytrace freespace
138  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
139  {
140  raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
141  }
142 
143  // place the new obstacles into a priority queue... each with a priority of zero to begin with
144  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
145  {
146  const Observation& obs = *it;
147 
148  const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
149 
150  double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
151 
152  for (unsigned int i = 0; i < cloud.points.size(); ++i)
153  {
154  // if the obstacle is too high or too far away from the robot we won't add it
155  if (cloud.points[i].z > max_obstacle_height_)
156  continue;
157 
158  // compute the squared distance from the hitpoint to the pointcloud's origin
159  double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
160  + (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
161  + (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
162 
163  // if the point is far enough away... we won't consider it
164  if (sq_dist >= sq_obstacle_range)
165  continue;
166 
167  // now we need to compute the map coordinates for the observation
168  unsigned int mx, my, mz;
169  if (cloud.points[i].z < origin_z_)
170  {
171  if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, origin_z_, mx, my, mz))
172  continue;
173  }
174  else if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z, mx, my, mz))
175  {
176  continue;
177  }
178 
179  // mark the cell in the voxel grid and check if we should also mark it in the costmap
180  if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
181  {
182  unsigned int index = getIndex(mx, my);
183 
184  costmap_[index] = LETHAL_OBSTACLE;
185  touch((double)cloud.points[i].x, (double)cloud.points[i].y, min_x, min_y, max_x, max_y);
186  }
187  }
188  }
189 
190  if (publish_voxel_)
191  {
192  costmap_2d::VoxelGrid grid_msg;
193  unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
194  grid_msg.size_x = voxel_grid_.sizeX();
195  grid_msg.size_y = voxel_grid_.sizeY();
196  grid_msg.size_z = voxel_grid_.sizeZ();
197  grid_msg.data.resize(size);
198  memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
199 
200  grid_msg.origin.x = origin_x_;
201  grid_msg.origin.y = origin_y_;
202  grid_msg.origin.z = origin_z_;
203 
204  grid_msg.resolutions.x = resolution_;
205  grid_msg.resolutions.y = resolution_;
206  grid_msg.resolutions.z = z_resolution_;
207  grid_msg.header.frame_id = global_frame_;
208  grid_msg.header.stamp = ros::Time::now();
209  voxel_pub_.publish(grid_msg);
210  }
211 
212  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
213 }
214 
215 void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
216 {
217  // get the cell coordinates of the center point of the window
218  unsigned int mx, my;
219  if (!worldToMap(wx, wy, mx, my))
220  return;
221 
222  // compute the bounds of the window
223  double start_x = wx - w_size_x / 2;
224  double start_y = wy - w_size_y / 2;
225  double end_x = start_x + w_size_x;
226  double end_y = start_y + w_size_y;
227 
228  // scale the window based on the bounds of the costmap
229  start_x = std::max(origin_x_, start_x);
230  start_y = std::max(origin_y_, start_y);
231 
232  end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
233  end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
234 
235  // get the map coordinates of the bounds of the window
236  unsigned int map_sx, map_sy, map_ex, map_ey;
237 
238  // check for legality just in case
239  if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
240  return;
241 
242  // we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
243  unsigned int index = getIndex(map_sx, map_sy);
244  unsigned char* current = &costmap_[index];
245  for (unsigned int j = map_sy; j <= map_ey; ++j)
246  {
247  for (unsigned int i = map_sx; i <= map_ex; ++i)
248  {
249  // if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
250  if (*current != LETHAL_OBSTACLE)
251  {
252  if (clear_no_info || *current != NO_INFORMATION)
253  {
254  *current = FREE_SPACE;
255  voxel_grid_.clearVoxelColumn(index);
256  }
257  }
258  current++;
259  index++;
260  }
261  current += size_x_ - (map_ex - map_sx) - 1;
262  index += size_x_ - (map_ex - map_sx) - 1;
263  }
264 }
265 
266 void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
267  double* max_x, double* max_y)
268 {
269  if (clearing_observation.cloud_->points.size() == 0)
270  return;
271 
272  double sensor_x, sensor_y, sensor_z;
273  double ox = clearing_observation.origin_.x;
274  double oy = clearing_observation.origin_.y;
275  double oz = clearing_observation.origin_.z;
276 
277  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
278  {
280  1.0,
281  "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
282  ox, oy, oz);
283  return;
284  }
285 
286  bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
287  if (publish_clearing_points)
288  {
289  clearing_endpoints_.points.clear();
290  clearing_endpoints_.points.reserve(clearing_observation.cloud_->points.size());
291  }
292 
293  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
294  double map_end_x = origin_x_ + getSizeInMetersX();
295  double map_end_y = origin_y_ + getSizeInMetersY();
296 
297  for (unsigned int i = 0; i < clearing_observation.cloud_->points.size(); ++i)
298  {
299  double wpx = clearing_observation.cloud_->points[i].x;
300  double wpy = clearing_observation.cloud_->points[i].y;
301  double wpz = clearing_observation.cloud_->points[i].z;
302 
303  double distance = dist(ox, oy, oz, wpx, wpy, wpz);
304  double scaling_fact = 1.0;
305  scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
306  wpx = scaling_fact * (wpx - ox) + ox;
307  wpy = scaling_fact * (wpy - oy) + oy;
308  wpz = scaling_fact * (wpz - oz) + oz;
309 
310  double a = wpx - ox;
311  double b = wpy - oy;
312  double c = wpz - oz;
313  double t = 1.0;
314 
315  // we can only raytrace to a maximum z height
316  if (wpz > max_obstacle_height_)
317  {
318  // we know we want the vector's z value to be max_z
319  t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
320  }
321  // and we can only raytrace down to the floor
322  else if (wpz < origin_z_)
323  {
324  // we know we want the vector's z value to be 0.0
325  t = std::min(t, (origin_z_ - oz) / c);
326  }
327 
328  // the minimum value to raytrace from is the origin
329  if (wpx < origin_x_)
330  {
331  t = std::min(t, (origin_x_ - ox) / a);
332  }
333  if (wpy < origin_y_)
334  {
335  t = std::min(t, (origin_y_ - oy) / b);
336  }
337 
338  // the maximum value to raytrace to is the end of the map
339  if (wpx > map_end_x)
340  {
341  t = std::min(t, (map_end_x - ox) / a);
342  }
343  if (wpy > map_end_y)
344  {
345  t = std::min(t, (map_end_y - oy) / b);
346  }
347 
348  wpx = ox + a * t;
349  wpy = oy + b * t;
350  wpz = oz + c * t;
351 
352  double point_x, point_y, point_z;
353  if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
354  {
355  unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
356 
357  // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
358  voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
359  unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
360  cell_raytrace_range);
361 
362  updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
363 
364  if (publish_clearing_points)
365  {
366  geometry_msgs::Point32 point;
367  point.x = wpx;
368  point.y = wpy;
369  point.z = wpz;
370  clearing_endpoints_.points.push_back(point);
371  }
372  }
373  }
374 
375  if (publish_clearing_points)
376  {
377  clearing_endpoints_.header.frame_id = global_frame_;
378  clearing_endpoints_.header.stamp = pcl_conversions::fromPCL(clearing_observation.cloud_->header).stamp;
379  clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
380 
381  clearing_endpoints_pub_.publish(clearing_endpoints_);
382  }
383 }
384 
385 void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
386 {
387  // project the new origin into the grid
388  int cell_ox, cell_oy;
389  cell_ox = int((new_origin_x - origin_x_) / resolution_);
390  cell_oy = int((new_origin_y - origin_y_) / resolution_);
391 
392  // compute the associated world coordinates for the origin cell
393  // beacuase we want to keep things grid-aligned
394  double new_grid_ox, new_grid_oy;
395  new_grid_ox = origin_x_ + cell_ox * resolution_;
396  new_grid_oy = origin_y_ + cell_oy * resolution_;
397 
398  // To save casting from unsigned int to int a bunch of times
399  int size_x = size_x_;
400  int size_y = size_y_;
401 
402  // we need to compute the overlap of the new and existing windows
403  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
404  lower_left_x = std::min(std::max(cell_ox, 0), size_x);
405  lower_left_y = std::min(std::max(cell_oy, 0), size_y);
406  upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
407  upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
408 
409  unsigned int cell_size_x = upper_right_x - lower_left_x;
410  unsigned int cell_size_y = upper_right_y - lower_left_y;
411 
412  // we need a map to store the obstacles in the window temporarily
413  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
414  unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
415  unsigned int* voxel_map = voxel_grid_.getData();
416 
417  // copy the local window in the costmap to the local map
418  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
419  copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
420  cell_size_y);
421 
422  // we'll reset our maps to unknown space if appropriate
423  resetMaps();
424 
425  // update the origin with the appropriate world coordinates
426  origin_x_ = new_grid_ox;
427  origin_y_ = new_grid_oy;
428 
429  // compute the starting cell location for copying data back in
430  int start_x = lower_left_x - cell_ox;
431  int start_y = lower_left_y - cell_oy;
432 
433  // now we want to copy the overlapping information back into the map, but in its new location
434  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
435  copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
436 
437  // make sure to clean up
438  delete[] local_map;
439  delete[] local_voxel_map;
440 }
441 
442 } // namespace costmap_2d
geometry_msgs::Point origin_
Definition: observation.h:97
#define ROS_WARN_THROTTLE(rate,...)
pcl::PointCloud< pcl::PointXYZ > * cloud_
Definition: observation.h:98
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
Stores an observation in terms of a point cloud and the origin of the source.
Definition: observation.h:47
static const unsigned char FREE_SPACE
Definition: cost_values.h:45
geometry_msgs::TransformStamped t
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
double distance(double x0, double y0, double x1, double y1)
Definition: costmap_math.h:58
Takes in point clouds from sensors, transforms them to the desired frame, and stores them...
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static const unsigned char LETHAL_OBSTACLE
Definition: cost_values.h:43
static const unsigned char NO_INFORMATION
Definition: cost_values.h:42
static Time now()
#define ROS_ASSERT(cond)
#define VOXEL_BITS
Definition: voxel_layer.cpp:42


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:42