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 
172 static const std::vector<Cell>& clearAndReturn(std::vector<Cell>& _cells)
173 {
174  _cells.clear();
175  return _cells;
176 }
177 
181 std::vector<Cell> FootprintHelper::getFootprintCells(double x, double y, double theta,
182  const std::vector<geometry_msgs::Point>& footprint_spec,
183  const costmap_2d::Costmap2D& costmap, bool fill)
184 {
185  std::vector<Cell> footprint_cells;
186 
187  //if we have no footprint... do nothing
188  if (footprint_spec.size() <= 1) {
189  unsigned int mx, my;
190  if (costmap.worldToMap(x, y, mx, my)) {
191  Cell center;
192  center.x = mx;
193  center.y = my;
194  footprint_cells.push_back(center);
195  }
196  return footprint_cells;
197  }
198 
199  //pre-compute cos and sin values
200  double cos_th = cos(theta);
201  double sin_th = sin(theta);
202  double new_x, new_y;
203  unsigned int x0, y0, x1, y1;
204  unsigned int last_index = footprint_spec.size() - 1;
205 
206  for (unsigned int i = 0; i < last_index; ++i) {
207  //find the cell coordinates of the first segment point
208  new_x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
209  new_y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
210  if(!costmap.worldToMap(new_x, new_y, x0, y0)) {
211  return clearAndReturn(footprint_cells);
212  }
213 
214  //find the cell coordinates of the second segment point
215  new_x = x + (footprint_spec[i + 1].x * cos_th - footprint_spec[i + 1].y * sin_th);
216  new_y = y + (footprint_spec[i + 1].x * sin_th + footprint_spec[i + 1].y * cos_th);
217  if (!costmap.worldToMap(new_x, new_y, x1, y1)) {
218  return clearAndReturn(footprint_cells);
219  }
220 
221  getLineCells(x0, x1, y0, y1, footprint_cells);
222  }
223 
224  //we need to close the loop, so we also have to raytrace from the last pt to first pt
225  new_x = x + (footprint_spec[last_index].x * cos_th - footprint_spec[last_index].y * sin_th);
226  new_y = y + (footprint_spec[last_index].x * sin_th + footprint_spec[last_index].y * cos_th);
227  if (!costmap.worldToMap(new_x, new_y, x0, y0)) {
228  return clearAndReturn(footprint_cells);
229  }
230  new_x = x + (footprint_spec[0].x * cos_th - footprint_spec[0].y * sin_th);
231  new_y = y + (footprint_spec[0].x * sin_th + footprint_spec[0].y * cos_th);
232  if(!costmap.worldToMap(new_x, new_y, x1, y1)) {
233  return clearAndReturn(footprint_cells);
234  }
235 
236  getLineCells(x0, x1, y0, y1, footprint_cells);
237 
238  if(fill) {
239  getFillCells(footprint_cells);
240  }
241 
242  return footprint_cells;
243 }
244 
245 } /* namespace mbf_costmap_nav */
costmap_2d::Costmap2D::worldToMap
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
footprint_helper.h
costmap_2d::Costmap2D
swap
void swap(Bag &a, Bag &b)
Cell::x
double x
mbf_costmap_nav
Definition: costmap_controller_execution.h:51
Cell
mbf_costmap_nav::FootprintHelper::getLineCells
static void getLineCells(int x0, int x1, int y0, int y1, std::vector< Cell > &pts)
Use Bresenham's algorithm to trace a line between two points in a grid.
Definition: footprint_helper.cpp:78
mbf_costmap_nav::FootprintHelper::getFillCells
static void getFillCells(std::vector< Cell > &footprint)
Fill the outline of a polygon, in this case the robot footprint, in a grid.
Definition: footprint_helper.cpp:150
mbf_costmap_nav::FootprintHelper::getFootprintCells
static std::vector< Cell > getFootprintCells(double x, double y, double theta, const 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.
Definition: footprint_helper.cpp:216
Cell::y
double y
mbf_costmap_nav::clearAndReturn
static const std::vector< Cell > & clearAndReturn(std::vector< Cell > &_cells)
Helper function which clears and returns the input.
Definition: footprint_helper.cpp:207


mbf_costmap_nav
Author(s): Sebastian Pütz
autogenerated on Wed Mar 2 2022 00:33:55