gradient_path.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
37 #include <nav_core2/exceptions.h>
39 #include <math.h>
40 #include <string>
41 
43 
44 using dlux_global_planner::HIGH_POTENTIAL;
45 
46 namespace dlux_plugins
47 {
48 void GradientPath::initialize(ros::NodeHandle& private_nh, dlux_global_planner::CostInterpreter::Ptr cost_interpreter)
49 {
50  cost_interpreter_ = cost_interpreter;
51  private_nh.param("step_size", step_size_, 0.5);
52  private_nh.param("lethal_cost", lethal_cost_, 250.0);
53 
54  // As a safeguard against infinitely iterating, we limit the number of steps we take, relative to the size of the
55  // costmap/potential grid. The maximum number of iterations is width * height * iteration_factor.
56  private_nh.param("iteration_factor", iteration_factor_, 4.0);
57 
58  // NavFn would not attempt to calculate a gradient if one of the neighboring cells had not been initialized.
59  // Setting this to true will replicate that behavior
60  private_nh.param("grid_step_near_high", grid_step_near_high_, false);
61 }
62 
63 nav_2d_msgs::Path2D GradientPath::getPath(const dlux_global_planner::PotentialGrid& potential_grid,
64  const geometry_msgs::Pose2D& start, const geometry_msgs::Pose2D& goal,
65  double& path_cost)
66 {
67  const nav_grid::NavGridInfo& info = potential_grid.getInfo();
68  if (gradx_.getInfo() != info)
69  {
70  gradx_.setInfo(info);
71  grady_.setInfo(info);
72  }
73  gradx_.reset();
74  grady_.reset();
75 
76  nav_2d_msgs::Path2D path;
77  path_cost = 0.0;
78 
79  double map_x, map_y;
80  worldToGrid(info, start.x, start.y, map_x, map_y);
81  // Running Variables
82  // index is the current index
83  // dx and dy represent the precise floating point coordinates within the cell [0, 1)
84  double x_int_part, y_int_part;
85  float dx = modf(map_x, &x_int_part),
86  dy = modf(map_y, &y_int_part);
87  nav_grid::Index index(static_cast<unsigned int>(x_int_part), static_cast<unsigned int>(y_int_part));
88 
89  // Variables to check for loop completion
90  worldToGrid(info, goal.x, goal.y, map_x, map_y);
91  double finish_x = floor(map_x),
92  finish_y = floor(map_y);
93  nav_grid::Index finish_index(static_cast<unsigned int>(finish_x), static_cast<unsigned int>(finish_y));
94 
95  unsigned int c = 0, max_iterations = static_cast<unsigned int>(potential_grid.size() * iteration_factor_);
96  geometry_msgs::Pose2D current;
97 
98  while (c++ < max_iterations)
99  {
100  current.x = index.x + dx;
101  current.y = index.y + dy;
102  path.poses.push_back(current);
103  path_cost += step_size_ * cost_interpreter_->getCost(index.x, index.y);
104 
105  // check if near goal
106  if (index == finish_index || (fabs(current.x - finish_x) <= step_size_ && fabs(current.y - finish_y) <= step_size_))
107  {
108  nav_2d_msgs::Path2D world_path = mapPathToWorldPath(path, potential_grid.getInfo());
109  world_path.poses.push_back(goal);
110  return world_path;
111  }
112 
113  ROS_DEBUG_NAMED("GradientPath", "xy: %.2f %.2f (%d %.2f, %d %.2f)", current.x, current.y,
114  index.x, dx, index.y, dy);
115 
116  // Check if the most recent pose, and two before the most recent pose are the same
117  bool oscillation_detected = false;
118  int npath = path.poses.size();
119  if (npath > 2 && path.poses[npath - 1].x == path.poses[npath - 3].x
120  && path.poses[npath - 1].y == path.poses[npath - 3].y)
121  {
122  oscillation_detected = true;
123  }
124 
125  /* We need to examine eight different neighboring cells.
126  * +----------+----------+----------+
127  * | index_nw | index_n | index_ne |
128  * +----------+----------+----------+
129  * | index_w | index | index_e |
130  * +----------+----------+----------+
131  * | index_sw | index_s | index_se |
132  * +----------+----------+----------+
133  *
134  * The original implementors of navfn unrolled the loops for interating over all the cells.
135  * We copy their technique here.
136  */
137  if (shouldGridStep(potential_grid, index) || oscillation_detected)
138  {
139  std::string error;
140  if (oscillation_detected)
141  error = "Oscillation detected.";
142  else
143  error = "High Potential Nearby.";
144  ROS_DEBUG_NAMED("GradientPath", "%s Following grid.", error.c_str());
145  index = gridStep(potential_grid, index);
146  dx = 0.0;
147  dy = 0.0;
148 
149  ROS_DEBUG_NAMED("GradientPath", "Potential: %0.1f position: %0.1f,%0.1f",
150  potential_grid(index), path.poses[npath - 1].x, path.poses[npath - 1].y);
151  }
152  // have a good gradient here
153  else
154  {
155  /* Calculate the gradient of four cells
156  * +----------+----------+----------+
157  * | | index_n | index_ne |
158  * +----------o----------o----------+
159  * | | index | index_e |
160  * +----------o----------o----------+
161  * | | | |
162  * +----------+----------+----------+
163  * Our position is somewhere in the index cell, relative to dx and dy
164  * The gradient we calculate is relative to the lower left corner of each cell (as marked by 'o's above)
165  * Thus, the precise graident we will follow is two-dimensional linear combination of four gradients
166  * surrounding our current cell.
167  */
168  nav_grid::Index index_e(index.x + 1, index.y),
169  index_n(index.x, index.y + 1),
170  index_ne(index.x + 1, index.y + 1);
171  calculateGradient(potential_grid, index);
172  calculateGradient(potential_grid, index_e);
173  calculateGradient(potential_grid, index_n);
174  calculateGradient(potential_grid, index_ne);
175 
176  // get interpolated gradient
177  float x1 = (1.0 - dx) * gradx_(index) + dx * gradx_(index_e);
178  float x2 = (1.0 - dx) * gradx_(index_n) + dx * gradx_(index_ne);
179  float x = (1.0 - dy) * x1 + dy * x2; // interpolated x
180  float y1 = (1.0 - dx) * grady_(index) + dx * grady_(index_e);
181  float y2 = (1.0 - dx) * grady_(index_n) + dx * grady_(index_ne);
182  float y = (1.0 - dy) * y1 + dy * y2; // interpolated y
183 
184  // show gradients
185  ROS_DEBUG_NAMED("GradientPath", "%0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f; final x=%.3f, y=%.3f",
186  gradx_(index), grady_(index), gradx_(index_e), grady_(index_e),
187  gradx_(index_n), grady_(index_n), gradx_(index_ne), grady_(index_ne), x, y);
188 
189  // check for zero gradient
190  if (x == 0.0 && y == 0.0)
191  {
192  // This generally shouldn't happen, but can if using a not well-formed potential function
193  // If there's no gradient, move along the grid.
194  index = gridStep(potential_grid, index);
195  dx = 0.0;
196  dy = 0.0;
197  continue;
198  }
199 
200  // move in the right direction
201  float ss = step_size_ / hypot(x, y);
202  dx += x * ss;
203  dy += y * ss;
204 
205  // check if we have moved out of the current cell
206  if (dx >= 1.0)
207  {
208  index.x++;
209  dx -= 1.0;
210  }
211  if (dx < 0.0)
212  {
213  index.x--;
214  dx += 1.0;
215  }
216  if (dy >= 1.0)
217  {
218  index.y++;
219  dy -= 1.0;
220  }
221  if (dy < 0.0)
222  {
223  index.y--;
224  dy += 1.0;
225  }
226  }
227 
228  ROS_DEBUG_NAMED("GradientPath", "Potential: %0.1f gradient: %0.1f,%0.1f position: %0.1f,%0.1f\n",
229  potential_grid(index), dx, dy, path.poses[npath - 1].x, path.poses[npath - 1].y);
230  }
231 
232  return path;
233 }
234 
235 bool GradientPath::shouldGridStep(const dlux_global_planner::PotentialGrid& potential_grid,
236  const nav_grid::Index& index)
237 {
238  bool near_edge = index.x == 0 || index.x >= potential_grid.getWidth() - 1 ||
239  index.y == 0 || index.y >= potential_grid.getHeight() - 1;
240  if (near_edge || !grid_step_near_high_)
241  return near_edge;
242 
243  // check for potentials at eight positions near cell
244  return potential_grid(index) >= HIGH_POTENTIAL ||
245  potential_grid(index.x + 1, index.y) >= HIGH_POTENTIAL ||
246  potential_grid(index.x - 1, index.y) >= HIGH_POTENTIAL ||
247  potential_grid(index.x, index.y + 1) >= HIGH_POTENTIAL ||
248  potential_grid(index.x, index.y - 1) >= HIGH_POTENTIAL ||
249  potential_grid(index.x + 1, index.y + 1) >= HIGH_POTENTIAL ||
250  potential_grid(index.x + 1, index.y - 1) >= HIGH_POTENTIAL ||
251  potential_grid(index.x - 1, index.y + 1) >= HIGH_POTENTIAL ||
252  potential_grid(index.x - 1, index.y - 1) >= HIGH_POTENTIAL;
253 }
254 
255 nav_grid::Index GradientPath::gridStep(const dlux_global_planner::PotentialGrid& potential_grid,
256  const nav_grid::Index& index)
257 {
258  // check eight neighbors to find the lowest
259  nav_grid::Index min_index = index;
260  float min_potential = potential_grid(index);
261 
262  // Unrolled loop
263 
264  // Check the south
265  if (index.y > 0)
266  {
267  nav_grid::Index index_s(index.x, index.y - 1);
268  if (potential_grid(index_s) < min_potential)
269  {
270  min_potential = potential_grid(index_s);
271  min_index = index_s;
272  }
273 
274  // check the south west
275  if (index.x > 0)
276  {
277  nav_grid::Index index_sw(index.x - 1, index.y - 1);
278  if (potential_grid(index_sw) < min_potential)
279  {
280  min_potential = potential_grid(index_sw);
281  min_index = index_sw;
282  }
283  }
284 
285  // check the south east
286  if (index.x < potential_grid.getWidth() - 1)
287  {
288  nav_grid::Index index_se(index.x + 1, index.y - 1);
289  if (potential_grid(index_se) < min_potential)
290  {
291  min_potential = potential_grid(index_se);
292  min_index = index_se;
293  }
294  }
295  }
296 
297  // check the west
298  if (index.x > 0)
299  {
300  nav_grid::Index index_w(index.x - 1, index.y);
301  if (potential_grid(index_w) < min_potential)
302  {
303  min_potential = potential_grid(index_w);
304  min_index = index_w;
305  }
306  }
307  // check the east
308  if (index.x < potential_grid.getWidth() - 1)
309  {
310  nav_grid::Index index_e(index.x + 1, index.y);
311  if (potential_grid(index_e) < min_potential)
312  {
313  min_potential = potential_grid(index_e);
314  min_index = index_e;
315  }
316  }
317 
318  // Check the north
319  if (index.y < potential_grid.getHeight() - 1)
320  {
321  nav_grid::Index index_n(index.x, index.y + 1);
322  if (potential_grid(index_n) < min_potential)
323  {
324  min_potential = potential_grid(index_n);
325  min_index = index_n;
326  }
327 
328  // check the north west
329  if (index.x > 0)
330  {
331  nav_grid::Index index_nw(index.x - 1, index.y + 1);
332  if (potential_grid(index_nw) < min_potential)
333  {
334  min_potential = potential_grid(index_nw);
335  min_index = index_nw;
336  }
337  }
338 
339  // check the north east
340  if (index.x < potential_grid.getWidth() - 1)
341  {
342  nav_grid::Index index_ne(index.x + 1, index.y + 1);
343  if (potential_grid(index_ne) < min_potential)
344  {
345  min_potential = potential_grid(index_ne);
346  min_index = index_ne;
347  }
348  }
349  }
350  if (min_index == index)
351  {
352  throw nav_core2::PlannerException("No path found. Local min.");
353  }
354  else if (potential_grid(min_index) >= HIGH_POTENTIAL)
355  {
356  throw nav_core2::PlannerException("No path found, high potential");
357  }
358  return min_index;
359 }
360 
361 void GradientPath::calculateGradient(const dlux_global_planner::PotentialGrid& potential_grid,
362  const nav_grid::Index& index)
363 {
364  // If non-zero, gradient already calculated, skip
365  if (gradx_(index) + grady_(index) > 0.0)
366  return;
367 
368  float cv = potential_grid(index);
369  float dx = 0.0;
370  float dy = 0.0;
371 
372  float south_p = index.y > 0 ? potential_grid(index.x, index.y - 1) : HIGH_POTENTIAL;
373  float north_p = index.y < potential_grid.getHeight() - 1 ? potential_grid(index.x, index.y + 1) : HIGH_POTENTIAL;
374  float west_p = index.x > 0 ? potential_grid(index.x - 1, index.y) : HIGH_POTENTIAL;
375  float east_p = index.x < potential_grid.getWidth() - 1 ? potential_grid(index.x + 1, index.y) : HIGH_POTENTIAL;
376 
377  // check if in an obstacle
378  if (cv >= HIGH_POTENTIAL)
379  {
380  if (west_p < HIGH_POTENTIAL)
381  dx = -lethal_cost_;
382  else if (east_p < HIGH_POTENTIAL)
383  dx = lethal_cost_;
384 
385  if (south_p < HIGH_POTENTIAL)
386  dy = -lethal_cost_;
387  else if (north_p < HIGH_POTENTIAL)
388  dy = lethal_cost_;
389  }
390  else // not in an obstacle
391  {
392  // dx calc, average to sides
393  if (west_p < HIGH_POTENTIAL)
394  dx += west_p - cv;
395  if (east_p < HIGH_POTENTIAL)
396  dx += cv - east_p;
397 
398  // dy calc, average to sides
399  if (south_p < HIGH_POTENTIAL)
400  dy += south_p - cv;
401  if (north_p < HIGH_POTENTIAL)
402  dy += cv - north_p;
403  }
404 
405  // normalize
406  float norm = hypot(dx, dy);
407  if (norm > 0)
408  {
409  norm = 1.0 / norm;
410  gradx_.setValue(index, norm * dx);
411  grady_.setValue(index, norm * dy);
412  }
413 }
414 
415 } // namespace dlux_plugins
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_DEBUG_NAMED(name,...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
unsigned int getHeight() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
std::shared_ptr< CostInterpreter > Ptr
unsigned int size() const
unsigned int getWidth() const
NavGridInfo getInfo() const
void worldToGrid(const NavGridInfo &info, double wx, double wy, double &mx, double &my)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
Traceback function that creates a smooth gradient from the start to the goal.
Definition: gradient_path.h:47


dlux_plugins
Author(s):
autogenerated on Wed Jun 26 2019 20:06:32