footprint_helper.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 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: TKruse
36  *********************************************************************/
37 
39 
40 namespace base_local_planner {
41 
43  // TODO Auto-generated constructor stub
44 
45 }
46 
48  // TODO Auto-generated destructor stub
49 }
50 
51 void FootprintHelper::getLineCells(int x0, int x1, int y0, int y1, std::vector<base_local_planner::Position2DInt>& pts) {
52  //Bresenham Ray-Tracing
53  int deltax = abs(x1 - x0); // The difference between the x's
54  int deltay = abs(y1 - y0); // The difference between the y's
55  int x = x0; // Start x off at the first pixel
56  int y = y0; // Start y off at the first pixel
57 
58  int xinc1, xinc2, yinc1, yinc2;
59  int den, num, numadd, numpixels;
60 
61  base_local_planner::Position2DInt pt;
62 
63  if (x1 >= x0) // The x-values are increasing
64  {
65  xinc1 = 1;
66  xinc2 = 1;
67  }
68  else // The x-values are decreasing
69  {
70  xinc1 = -1;
71  xinc2 = -1;
72  }
73 
74  if (y1 >= y0) // The y-values are increasing
75  {
76  yinc1 = 1;
77  yinc2 = 1;
78  }
79  else // The y-values are decreasing
80  {
81  yinc1 = -1;
82  yinc2 = -1;
83  }
84 
85  if (deltax >= deltay) // There is at least one x-value for every y-value
86  {
87  xinc1 = 0; // Don't change the x when numerator >= denominator
88  yinc2 = 0; // Don't change the y for every iteration
89  den = deltax;
90  num = deltax / 2;
91  numadd = deltay;
92  numpixels = deltax; // There are more x-values than y-values
93  }
94  else // There is at least one y-value for every x-value
95  {
96  xinc2 = 0; // Don't change the x for every iteration
97  yinc1 = 0; // Don't change the y when numerator >= denominator
98  den = deltay;
99  num = deltay / 2;
100  numadd = deltax;
101  numpixels = deltay; // There are more y-values than x-values
102  }
103 
104  for (int curpixel = 0; curpixel <= numpixels; curpixel++)
105  {
106  pt.x = x; //Draw the current pixel
107  pt.y = y;
108  pts.push_back(pt);
109 
110  num += numadd; // Increase the numerator by the top of the fraction
111  if (num >= den) // Check if numerator >= denominator
112  {
113  num -= den; // Calculate the new numerator value
114  x += xinc1; // Change the x as appropriate
115  y += yinc1; // Change the y as appropriate
116  }
117  x += xinc2; // Change the x as appropriate
118  y += yinc2; // Change the y as appropriate
119  }
120 }
121 
122 
123 void FootprintHelper::getFillCells(std::vector<base_local_planner::Position2DInt>& footprint){
124  //quick bubble sort to sort pts by x
125  base_local_planner::Position2DInt swap, pt;
126  unsigned int i = 0;
127  while (i < footprint.size() - 1) {
128  if (footprint[i].x > footprint[i + 1].x) {
129  swap = footprint[i];
130  footprint[i] = footprint[i + 1];
131  footprint[i + 1] = swap;
132  if(i > 0) {
133  --i;
134  }
135  } else {
136  ++i;
137  }
138  }
139 
140  i = 0;
141  base_local_planner::Position2DInt min_pt;
142  base_local_planner::Position2DInt max_pt;
143  unsigned int min_x = footprint[0].x;
144  unsigned int max_x = footprint[footprint.size() -1].x;
145  //walk through each column and mark cells inside the footprint
146  for (unsigned int x = min_x; x <= max_x; ++x) {
147  if (i >= footprint.size() - 1) {
148  break;
149  }
150  if (footprint[i].y < footprint[i + 1].y) {
151  min_pt = footprint[i];
152  max_pt = footprint[i + 1];
153  } else {
154  min_pt = footprint[i + 1];
155  max_pt = footprint[i];
156  }
157 
158  i += 2;
159  while (i < footprint.size() && footprint[i].x == x) {
160  if(footprint[i].y < min_pt.y) {
161  min_pt = footprint[i];
162  } else if(footprint[i].y > max_pt.y) {
163  max_pt = footprint[i];
164  }
165  ++i;
166  }
167 
168  //loop though cells in the column
169  for (unsigned int y = min_pt.y; y < max_pt.y; ++y) {
170  pt.x = x;
171  pt.y = y;
172  footprint.push_back(pt);
173  }
174  }
175 }
176 
180 std::vector<base_local_planner::Position2DInt> FootprintHelper::getFootprintCells(
181  Eigen::Vector3f pos,
182  std::vector<geometry_msgs::Point> footprint_spec,
183  const costmap_2d::Costmap2D& costmap,
184  bool fill){
185  double x_i = pos[0];
186  double y_i = pos[1];
187  double theta_i = pos[2];
188  std::vector<base_local_planner::Position2DInt> footprint_cells;
189 
190  //if we have no footprint... do nothing
191  if (footprint_spec.size() <= 1) {
192  unsigned int mx, my;
193  if (costmap.worldToMap(x_i, y_i, mx, my)) {
194  Position2DInt center;
195  center.x = mx;
196  center.y = my;
197  footprint_cells.push_back(center);
198  }
199  return footprint_cells;
200  }
201 
202  //pre-compute cos and sin values
203  double cos_th = cos(theta_i);
204  double sin_th = sin(theta_i);
205  double new_x, new_y;
206  unsigned int x0, y0, x1, y1;
207  unsigned int last_index = footprint_spec.size() - 1;
208 
209  for (unsigned int i = 0; i < last_index; ++i) {
210  //find the cell coordinates of the first segment point
211  new_x = x_i + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
212  new_y = y_i + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
213  if(!costmap.worldToMap(new_x, new_y, x0, y0)) {
214  return footprint_cells;
215  }
216 
217  //find the cell coordinates of the second segment point
218  new_x = x_i + (footprint_spec[i + 1].x * cos_th - footprint_spec[i + 1].y * sin_th);
219  new_y = y_i + (footprint_spec[i + 1].x * sin_th + footprint_spec[i + 1].y * cos_th);
220  if (!costmap.worldToMap(new_x, new_y, x1, y1)) {
221  return footprint_cells;
222  }
223 
224  getLineCells(x0, x1, y0, y1, footprint_cells);
225  }
226 
227  //we need to close the loop, so we also have to raytrace from the last pt to first pt
228  new_x = x_i + (footprint_spec[last_index].x * cos_th - footprint_spec[last_index].y * sin_th);
229  new_y = y_i + (footprint_spec[last_index].x * sin_th + footprint_spec[last_index].y * cos_th);
230  if (!costmap.worldToMap(new_x, new_y, x0, y0)) {
231  return footprint_cells;
232  }
233  new_x = x_i + (footprint_spec[0].x * cos_th - footprint_spec[0].y * sin_th);
234  new_y = y_i + (footprint_spec[0].x * sin_th + footprint_spec[0].y * cos_th);
235  if(!costmap.worldToMap(new_x, new_y, x1, y1)) {
236  return footprint_cells;
237  }
238 
239  getLineCells(x0, x1, y0, y1, footprint_cells);
240 
241  if(fill) {
242  getFillCells(footprint_cells);
243  }
244 
245  return footprint_cells;
246 }
247 
248 } /* namespace base_local_planner */
std::vector< base_local_planner::Position2DInt > getFootprintCells(Eigen::Vector3f pos, std::vector< geometry_msgs::Point > footprint_spec, const costmap_2d::Costmap2D &, bool fill)
Used to get the cells that make up the footprint of the robot.
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
void getFillCells(std::vector< base_local_planner::Position2DInt > &footprint)
Fill the outline of a polygon, in this case the robot footprint, in a grid.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
void getLineCells(int x0, int x1, int y0, int y1, std::vector< base_local_planner::Position2DInt > &pts)
Use Bresenham&#39;s algorithm to trace a line between two points in a grid.
void swap(scoped_ptr< T > &, scoped_ptr< T > &)
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)


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