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


mbf_costmap_nav
Author(s): Sebastian Pütz
autogenerated on Fri Nov 6 2020 03:56:29