voxel_grid_model.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, 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 the Willow Garage 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 *********************************************************************/
38 
39 using namespace std;
40 using namespace costmap_2d;
41 
42 namespace base_local_planner {
43  VoxelGridModel::VoxelGridModel(double size_x, double size_y, double size_z, double xy_resolution, double z_resolution,
44  double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range) :
45  obstacle_grid_(size_x, size_y, size_z), xy_resolution_(xy_resolution), z_resolution_(z_resolution),
46  origin_x_(origin_x), origin_y_(origin_y), origin_z_(origin_z),
47  max_z_(max_z), sq_obstacle_range_(obstacle_range * obstacle_range) {}
48 
49  double VoxelGridModel::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
50  double inscribed_radius, double circumscribed_radius){
51  if(footprint.size() < 3)
52  return -1.0;
53 
54  //now we really have to lay down the footprint in the costmap grid
55  unsigned int x0, x1, y0, y1;
56  double line_cost = 0.0;
57 
58  //we need to rasterize each line in the footprint
59  for(unsigned int i = 0; i < footprint.size() - 1; ++i){
60  //get the cell coord of the first point
61  if(!worldToMap2D(footprint[i].x, footprint[i].y, x0, y0))
62  return -1.0;
63 
64  //get the cell coord of the second point
65  if(!worldToMap2D(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
66  return -1.0;
67 
68  line_cost = lineCost(x0, x1, y0, y1);
69 
70  //if there is an obstacle that hits the line... we know that we can return false right away
71  if(line_cost < 0)
72  return -1.0;
73  }
74 
75  //we also need to connect the first point in the footprint to the last point
76  //get the cell coord of the last point
77  if(!worldToMap2D(footprint.back().x, footprint.back().y, x0, y0))
78  return -1.0;
79 
80  //get the cell coord of the first point
81  if(!worldToMap2D(footprint.front().x, footprint.front().y, x1, y1))
82  return -1.0;
83 
84  line_cost = lineCost(x0, x1, y0, y1);
85 
86  if(line_cost < 0)
87  return -1.0;
88 
89  //if all line costs are legal... then we can return that the footprint is legal
90  return 0.0;
91  }
92 
93  //calculate the cost of a ray-traced line
94  double VoxelGridModel::lineCost(int x0, int x1,
95  int y0, int y1){
96  //Bresenham Ray-Tracing
97  int deltax = abs(x1 - x0); // The difference between the x's
98  int deltay = abs(y1 - y0); // The difference between the y's
99  int x = x0; // Start x off at the first pixel
100  int y = y0; // Start y off at the first pixel
101 
102  int xinc1, xinc2, yinc1, yinc2;
103  int den, num, numadd, numpixels;
104 
105  double line_cost = 0.0;
106  double point_cost = -1.0;
107 
108  if (x1 >= x0) // The x-values are increasing
109  {
110  xinc1 = 1;
111  xinc2 = 1;
112  }
113  else // The x-values are decreasing
114  {
115  xinc1 = -1;
116  xinc2 = -1;
117  }
118 
119  if (y1 >= y0) // The y-values are increasing
120  {
121  yinc1 = 1;
122  yinc2 = 1;
123  }
124  else // The y-values are decreasing
125  {
126  yinc1 = -1;
127  yinc2 = -1;
128  }
129 
130  if (deltax >= deltay) // There is at least one x-value for every y-value
131  {
132  xinc1 = 0; // Don't change the x when numerator >= denominator
133  yinc2 = 0; // Don't change the y for every iteration
134  den = deltax;
135  num = deltax / 2;
136  numadd = deltay;
137  numpixels = deltax; // There are more x-values than y-values
138  }
139  else // There is at least one y-value for every x-value
140  {
141  xinc2 = 0; // Don't change the x for every iteration
142  yinc1 = 0; // Don't change the y when numerator >= denominator
143  den = deltay;
144  num = deltay / 2;
145  numadd = deltax;
146  numpixels = deltay; // There are more y-values than x-values
147  }
148 
149  for (int curpixel = 0; curpixel <= numpixels; curpixel++)
150  {
151  point_cost = pointCost(x, y); //Score the current point
152 
153  if(point_cost < 0)
154  return -1;
155 
156  if(line_cost < point_cost)
157  line_cost = point_cost;
158 
159  num += numadd; // Increase the numerator by the top of the fraction
160  if (num >= den) // Check if numerator >= denominator
161  {
162  num -= den; // Calculate the new numerator value
163  x += xinc1; // Change the x as appropriate
164  y += yinc1; // Change the y as appropriate
165  }
166  x += xinc2; // Change the x as appropriate
167  y += yinc2; // Change the y as appropriate
168  }
169 
170  return line_cost;
171  }
172 
173  double VoxelGridModel::pointCost(int x, int y){
174  //if the cell is in an obstacle the path is invalid
175  if(obstacle_grid_.getVoxelColumn(x, y)){
176  return -1;
177  }
178 
179  return 1;
180  }
181 
182  void VoxelGridModel::updateWorld(const std::vector<geometry_msgs::Point>& footprint,
183  const vector<Observation>& observations, const vector<PlanarLaserScan>& laser_scans){
184 
185  //remove points in the laser scan boundry
186  for(unsigned int i = 0; i < laser_scans.size(); ++i)
187  removePointsInScanBoundry(laser_scans[i], 10.0);
188 
189  //iterate through all observations and update the grid
190  for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
191  const Observation& obs = *it;
192  const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
193  for(unsigned int i = 0; i < cloud.size(); ++i){
194  //filter out points that are too high
195  if(cloud[i].z > max_z_)
196  continue;
197 
198  //compute the squared distance from the hitpoint to the pointcloud's origin
199  double sq_dist = (cloud[i].x - obs.origin_.x) * (cloud[i].x - obs.origin_.x)
200  + (cloud[i].y - obs.origin_.y) * (cloud[i].y - obs.origin_.y)
201  + (cloud[i].z - obs.origin_.z) * (cloud[i].z - obs.origin_.z);
202 
203  if(sq_dist >= sq_obstacle_range_)
204  continue;
205 
206  //insert the point
207  insert(cloud[i]);
208  }
209  }
210 
211  //remove the points that are in the footprint of the robot
212  //removePointsInPolygon(footprint);
213  }
214 
215  void VoxelGridModel::removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range){
216  if(laser_scan.cloud.points.size() == 0)
217  return;
218 
219  unsigned int sensor_x, sensor_y, sensor_z;
220  double ox = laser_scan.origin.x;
221  double oy = laser_scan.origin.y;
222  double oz = laser_scan.origin.z;
223 
224  if(!worldToMap3D(ox, oy, oz, sensor_x, sensor_y, sensor_z))
225  return;
226 
227  for(unsigned int i = 0; i < laser_scan.cloud.points.size(); ++i){
228  double wpx = laser_scan.cloud.points[i].x;
229  double wpy = laser_scan.cloud.points[i].y;
230  double wpz = laser_scan.cloud.points[i].z;
231 
232  double distance = dist(ox, oy, oz, wpx, wpy, wpz);
233  double scaling_fact = raytrace_range / distance;
234  scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
235  wpx = scaling_fact * (wpx - ox) + ox;
236  wpy = scaling_fact * (wpy - oy) + oy;
237  wpz = scaling_fact * (wpz - oz) + oz;
238 
239  //we can only raytrace to a maximum z height
240  if(wpz >= max_z_){
241  //we know we want the vector's z value to be max_z
242  double a = wpx - ox;
243  double b = wpy - oy;
244  double c = wpz - oz;
245  double t = (max_z_ - .01 - oz) / c;
246  wpx = ox + a * t;
247  wpy = oy + b * t;
248  wpz = oz + c * t;
249  }
250  //and we can only raytrace down to the floor
251  else if(wpz < 0.0){
252  //we know we want the vector's z value to be 0.0
253  double a = wpx - ox;
254  double b = wpy - oy;
255  double c = wpz - oz;
256  double t = (0.0 - oz) / c;
257  wpx = ox + a * t;
258  wpy = oy + b * t;
259  wpz = oz + c * t;
260  }
261 
262  unsigned int point_x, point_y, point_z;
263  if(worldToMap3D(wpx, wpy, wpz, point_x, point_y, point_z)){
264  obstacle_grid_.clearVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
265  }
266  }
267  }
268 
269  void VoxelGridModel::getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud){
270  for(unsigned int i = 0; i < obstacle_grid_.sizeX(); ++i){
271  for(unsigned int j = 0; j < obstacle_grid_.sizeY(); ++j){
272  for(unsigned int k = 0; k < obstacle_grid_.sizeZ(); ++k){
273  if(obstacle_grid_.getVoxel(i, j, k)){
274  double wx, wy, wz;
275  mapToWorld3D(i, j, k, wx, wy, wz);
276  pcl::PointXYZ pt;
277  pt.x = wx;
278  pt.y = wy;
279  pt.z = wz;
280  cloud.points.push_back(pt);
281  }
282  }
283  }
284  }
285  }
286 
287 };
double sq_obstacle_range_
The square distance at which we no longer add obstacles to the grid.
geometry_msgs::Point origin_
double pointCost(int x, int y)
Checks the cost of a point in the costmap.
VoxelStatus getVoxelColumn(unsigned int x, unsigned int y, unsigned int unknown_threshold=0, unsigned int marked_threshold=0)
pcl::PointCloud< pcl::PointXYZ > * cloud_
bool worldToMap3D(double wx, double wy, double wz, unsigned int &mx, unsigned int &my, unsigned int &mz)
VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z)
TFSIMD_FORCE_INLINE const tfScalar & y() const
geometry_msgs::TransformStamped t
void updateWorld(const std::vector< geometry_msgs::Point > &footprint, const std::vector< costmap_2d::Observation > &observations, const std::vector< PlanarLaserScan > &laser_scans)
The costmap already keeps track of world observations, so for this world model this method does nothi...
double distance(double x0, double y0, double x1, double y1)
unsigned int sizeY()
void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double &wx, double &wy, double &wz)
double lineCost(int x0, int x1, int y0, int y1)
Rasterizes a line in the costmap grid and checks for collisions.
double max_z_
The height cutoff for adding points as obstacles.
unsigned int sizeX()
bool worldToMap2D(double wx, double wy, unsigned int &mx, unsigned int &my)
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)
Checks if any obstacles in the voxel grid lie inside a convex footprint that is rasterized into the g...
TFSIMD_FORCE_INLINE const tfScalar & z() const
void getPoints(pcl::PointCloud< pcl::PointXYZ > &cloud)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
voxel_grid::VoxelGrid obstacle_grid_
void removePointsInScanBoundry(const PlanarLaserScan &laser_scan, double raytrace_range)
double dist(double x0, double y0, double z0, double x1, double y1, double z1)
Stores a scan from a planar laser that can be used to clear freespace.
unsigned int sizeZ()
void clearVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length=UINT_MAX)


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49