gradient_path.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  *********************************************************************/
39 #include <algorithm>
40 #include <stdio.h>
42 
43 namespace global_planner {
44 
46  Traceback(p_calc), pathStep_(0.5) {
47  gradx_ = grady_ = NULL;
48 }
49 
51 
52  if (gradx_)
53  delete[] gradx_;
54  if (grady_)
55  delete[] grady_;
56 }
57 
58 void GradientPath::setSize(int xs, int ys) {
59  Traceback::setSize(xs, ys);
60  if (gradx_)
61  delete[] gradx_;
62  if (grady_)
63  delete[] grady_;
64  gradx_ = new float[xs * ys];
65  grady_ = new float[xs * ys];
66 }
67 
68 bool GradientPath::getPath(float* potential, double start_x, double start_y, double goal_x, double goal_y, std::vector<std::pair<float, float> >& path) {
69  std::pair<float, float> current;
70  int stc = getIndex(goal_x, goal_y);
71 
72  // set up offset
73  float dx = goal_x - (int)goal_x;
74  float dy = goal_y - (int)goal_y;
75  int ns = xs_ * ys_;
76  memset(gradx_, 0, ns * sizeof(float));
77  memset(grady_, 0, ns * sizeof(float));
78 
79  int c = 0;
80  while (c++<ns*4) {
81  // check if near goal
82  double nx = stc % xs_ + dx, ny = stc / xs_ + dy;
83 
84  if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .5) {
85  current.first = start_x;
86  current.second = start_y;
87  path.push_back(current);
88  return true;
89  }
90 
91  if (stc < xs_ || stc > xs_ * ys_ - xs_) // would be out of bounds
92  {
93  printf("[PathCalc] Out of bounds\n");
94  return false;
95  }
96 
97  current.first = nx;
98  current.second = ny;
99 
100  //ROS_INFO("%d %d | %f %f ", stc%xs_, stc/xs_, dx, dy);
101 
102  path.push_back(current);
103 
104  bool oscillation_detected = false;
105  int npath = path.size();
106  if (npath > 2 && path[npath - 1].first == path[npath - 3].first
107  && path[npath - 1].second == path[npath - 3].second) {
108  ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
109  oscillation_detected = true;
110  }
111 
112  int stcnx = stc + xs_;
113  int stcpx = stc - xs_;
114 
115  // check for potentials at eight positions near cell
116  if (potential[stc] >= POT_HIGH || potential[stc + 1] >= POT_HIGH || potential[stc - 1] >= POT_HIGH
117  || potential[stcnx] >= POT_HIGH || potential[stcnx + 1] >= POT_HIGH || potential[stcnx - 1] >= POT_HIGH
118  || potential[stcpx] >= POT_HIGH || potential[stcpx + 1] >= POT_HIGH || potential[stcpx - 1] >= POT_HIGH
119  || oscillation_detected) {
120  ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potential[stc], (int) path.size());
121  // check eight neighbors to find the lowest
122  int minc = stc;
123  int minp = potential[stc];
124  int st = stcpx - 1;
125  if (potential[st] < minp) {
126  minp = potential[st];
127  minc = st;
128  }
129  st++;
130  if (potential[st] < minp) {
131  minp = potential[st];
132  minc = st;
133  }
134  st++;
135  if (potential[st] < minp) {
136  minp = potential[st];
137  minc = st;
138  }
139  st = stc - 1;
140  if (potential[st] < minp) {
141  minp = potential[st];
142  minc = st;
143  }
144  st = stc + 1;
145  if (potential[st] < minp) {
146  minp = potential[st];
147  minc = st;
148  }
149  st = stcnx - 1;
150  if (potential[st] < minp) {
151  minp = potential[st];
152  minc = st;
153  }
154  st++;
155  if (potential[st] < minp) {
156  minp = potential[st];
157  minc = st;
158  }
159  st++;
160  if (potential[st] < minp) {
161  minp = potential[st];
162  minc = st;
163  }
164  stc = minc;
165  dx = 0;
166  dy = 0;
167 
168  //ROS_DEBUG("[Path] Pot: %0.1f pos: %0.1f,%0.1f",
169  // potential[stc], path[npath-1].first, path[npath-1].second);
170 
171  if (potential[stc] >= POT_HIGH) {
172  ROS_DEBUG("[PathCalc] No path found, high potential");
173  //savemap("navfn_highpot");
174  return 0;
175  }
176  }
177 
178  // have a good gradient here
179  else {
180 
181  // get grad at four positions near cell
182  gradCell(potential, stc);
183  gradCell(potential, stc + 1);
184  gradCell(potential, stcnx);
185  gradCell(potential, stcnx + 1);
186 
187  // get interpolated gradient
188  float x1 = (1.0 - dx) * gradx_[stc] + dx * gradx_[stc + 1];
189  float x2 = (1.0 - dx) * gradx_[stcnx] + dx * gradx_[stcnx + 1];
190  float x = (1.0 - dy) * x1 + dy * x2; // interpolated x
191  float y1 = (1.0 - dx) * grady_[stc] + dx * grady_[stc + 1];
192  float y2 = (1.0 - dx) * grady_[stcnx] + dx * grady_[stcnx + 1];
193  float y = (1.0 - dy) * y1 + dy * y2; // interpolated y
194 
195  // show gradients
196  ROS_DEBUG(
197  "[Path] %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f; final x=%.3f, y=%.3f\n", gradx_[stc], grady_[stc], gradx_[stc+1], grady_[stc+1], gradx_[stcnx], grady_[stcnx], gradx_[stcnx+1], grady_[stcnx+1], x, y);
198 
199  // check for zero gradient, failed
200  if (x == 0.0 && y == 0.0) {
201  ROS_DEBUG("[PathCalc] Zero gradient");
202  return 0;
203  }
204 
205  // move in the right direction
206  float ss = pathStep_ / hypot(x, y);
207  dx += x * ss;
208  dy += y * ss;
209 
210  // check for overflow
211  if (dx > 1.0) {
212  stc++;
213  dx -= 1.0;
214  }
215  if (dx < -1.0) {
216  stc--;
217  dx += 1.0;
218  }
219  if (dy > 1.0) {
220  stc += xs_;
221  dy -= 1.0;
222  }
223  if (dy < -1.0) {
224  stc -= xs_;
225  dy += 1.0;
226  }
227 
228  }
229 
230  //printf("[Path] Pot: %0.1f grad: %0.1f,%0.1f pos: %0.1f,%0.1f\n",
231  // potential[stc], dx, dy, path[npath-1].first, path[npath-1].second);
232  }
233 
234  return false;
235 }
236 
237 /*
238  int
239  NavFn::calcPath(int n, int *st)
240  {
241  // set up start position at cell
242  // st is always upper left corner for 4-point bilinear interpolation
243  if (st == NULL) st = start;
244  int stc = st[1]*nx + st[0];
245 
246  // go for <n> cycles at most
247  for (int i=0; i<n; i++)
248  {
249 
250 
251 
252  }
253 
254  // return npath; // out of cycles, return failure
255  ROS_DEBUG("[PathCalc] No path found, path too long");
256  //savemap("navfn_pathlong");
257  return 0; // out of cycles, return failure
258  }
259  */
260 
261 //
262 // gradient calculations
263 //
264 // calculate gradient at a cell
265 // positive value are to the right and down
266 float GradientPath::gradCell(float* potential, int n) {
267  if (gradx_[n] + grady_[n] > 0.0) // check this cell
268  return 1.0;
269 
270  if (n < xs_ || n > xs_ * ys_ - xs_) // would be out of bounds
271  return 0.0;
272  float cv = potential[n];
273  float dx = 0.0;
274  float dy = 0.0;
275 
276  // check for in an obstacle
277  if (cv >= POT_HIGH) {
278  if (potential[n - 1] < POT_HIGH)
279  dx = -lethal_cost_;
280  else if (potential[n + 1] < POT_HIGH)
281  dx = lethal_cost_;
282 
283  if (potential[n - xs_] < POT_HIGH)
284  dy = -lethal_cost_;
285  else if (potential[n + xs_] < POT_HIGH)
286  dy = lethal_cost_;
287  }
288 
289  else // not in an obstacle
290  {
291  // dx calc, average to sides
292  if (potential[n - 1] < POT_HIGH)
293  dx += potential[n - 1] - cv;
294  if (potential[n + 1] < POT_HIGH)
295  dx += cv - potential[n + 1];
296 
297  // dy calc, average to sides
298  if (potential[n - xs_] < POT_HIGH)
299  dy += potential[n - xs_] - cv;
300  if (potential[n + xs_] < POT_HIGH)
301  dy += cv - potential[n + xs_];
302  }
303 
304  // normalize
305  float norm = hypot(dx, dy);
306  if (norm > 0) {
307  norm = 1.0 / norm;
308  gradx_[n] = norm * dx;
309  grady_[n] = norm * dy;
310  }
311  return norm;
312 }
313 
314 } //end namespace global_planner
315 
void setSize(int xs, int ys)
unsigned char lethal_cost_
Definition: traceback.h:62
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
float gradCell(float *potential, int n)
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define POT_HIGH
Definition: planner_core.h:40
GradientPath(PotentialCalculator *p_calc)
int getIndex(int x, int y)
Definition: traceback.h:54
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual void setSize(int xs, int ys)
Definition: traceback.h:50
bool getPath(float *potential, double start_x, double start_y, double end_x, double end_y, std::vector< std::pair< float, float > > &path)
#define ROS_DEBUG(...)


global_planner
Author(s): David Lu!!
autogenerated on Thu Jan 21 2021 04:06:07