dijkstra.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 namespace global_planner {
41 
43  Expander(p_calc, nx, ny), pending_(NULL), precise_(false) {
44  // priority buffers
45  buffer1_ = new int[PRIORITYBUFSIZE];
46  buffer2_ = new int[PRIORITYBUFSIZE];
47  buffer3_ = new int[PRIORITYBUFSIZE];
48 
50 }
51 
53  delete[] buffer1_;
54  delete[] buffer2_;
55  delete[] buffer3_;
56  if (pending_)
57  delete[] pending_;
58 }
59 
60 //
61 // Set/Reset map size
62 //
63 void DijkstraExpansion::setSize(int xs, int ys) {
64  Expander::setSize(xs, ys);
65  if (pending_)
66  delete[] pending_;
67 
68  pending_ = new bool[ns_];
69  memset(pending_, 0, ns_ * sizeof(bool));
70 }
71 
72 //
73 // main propagation function
74 // Dijkstra method, breadth-first
75 // runs for a specified number of cycles,
76 // or until it runs out of cells to update,
77 // or until the Start cell is found (atStart = true)
78 //
79 
80 bool DijkstraExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
81  int cycles, float* potential) {
82  cells_visited_ = 0;
83  // priority buffers
86  currentEnd_ = 0;
88  nextEnd_ = 0;
90  overEnd_ = 0;
91  memset(pending_, 0, ns_ * sizeof(bool));
92  std::fill(potential, potential + ns_, POT_HIGH);
93 
94  // set goal
95  int k = toIndex(start_x, start_y);
96 
97  if(precise_)
98  {
99  double dx = start_x - (int)start_x, dy = start_y - (int)start_y;
100  dx = floorf(dx * 100 + 0.5) / 100;
101  dy = floorf(dy * 100 + 0.5) / 100;
102  potential[k] = neutral_cost_ * 2 * dx * dy;
103  potential[k+1] = neutral_cost_ * 2 * (1-dx)*dy;
104  potential[k+nx_] = neutral_cost_*2*dx*(1-dy);
105  potential[k+nx_+1] = neutral_cost_*2*(1-dx)*(1-dy);//*/
106 
107  push_cur(k+2);
108  push_cur(k-1);
109  push_cur(k+nx_-1);
110  push_cur(k+nx_+2);
111 
112  push_cur(k-nx_);
113  push_cur(k-nx_+1);
114  push_cur(k+nx_*2);
115  push_cur(k+nx_*2+1);
116  }else{
117  potential[k] = 0;
118  push_cur(k+1);
119  push_cur(k-1);
120  push_cur(k-nx_);
121  push_cur(k+nx_);
122  }
123 
124  int nwv = 0; // max priority block size
125  int nc = 0; // number of cells put into priority blocks
126  int cycle = 0; // which cycle we're on
127 
128  // set up start cell
129  int startCell = toIndex(end_x, end_y);
130 
131  for (; cycle < cycles; cycle++) // go for this many cycles, unless interrupted
132  {
133  //
134  if (currentEnd_ == 0 && nextEnd_ == 0) // priority blocks empty
135  return false;
136 
137  // stats
138  nc += currentEnd_;
139  if (currentEnd_ > nwv)
140  nwv = currentEnd_;
141 
142  // reset pending_ flags on current priority buffer
143  int *pb = currentBuffer_;
144  int i = currentEnd_;
145  while (i-- > 0)
146  pending_[*(pb++)] = false;
147 
148  // process current priority buffer
149  pb = currentBuffer_;
150  i = currentEnd_;
151  while (i-- > 0)
152  updateCell(costs, potential, *pb++);
153 
154  // swap priority blocks currentBuffer_ <=> nextBuffer_
156  nextEnd_ = 0;
157  pb = currentBuffer_; // swap buffers
159  nextBuffer_ = pb;
160 
161  // see if we're done with this priority level
162  if (currentEnd_ == 0) {
163  threshold_ += priorityIncrement_; // increment priority threshold
164  currentEnd_ = overEnd_; // set current to overflow block
165  overEnd_ = 0;
166  pb = currentBuffer_; // swap buffers
168  overBuffer_ = pb;
169  }
170 
171  // check if we've hit the Start cell
172  if (potential[startCell] < POT_HIGH)
173  break;
174  }
175  //ROS_INFO("CYCLES %d/%d ", cycle, cycles);
176  if (cycle < cycles)
177  return true; // finished up here
178  else
179  return false;
180 }
181 
182 //
183 // Critical function: calculate updated potential value of a cell,
184 // given its neighbors' values
185 // Planar-wave update calculation from two lowest neighbors in a 4-grid
186 // Quadratic approximation to the interpolated value
187 // No checking of bounds here, this function should be fast
188 //
189 
190 #define INVSQRT2 0.707106781
191 
192 inline void DijkstraExpansion::updateCell(unsigned char* costs, float* potential, int n) {
193  cells_visited_++;
194 
195  // do planar wave update
196  float c = getCost(costs, n);
197  if (c >= lethal_cost_) // don't propagate into obstacles
198  return;
199 
200  float pot = p_calc_->calculatePotential(potential, c, n);
201 
202  // now add affected neighbors to priority blocks
203  if (pot < potential[n]) {
204  float le = INVSQRT2 * (float)getCost(costs, n - 1);
205  float re = INVSQRT2 * (float)getCost(costs, n + 1);
206  float ue = INVSQRT2 * (float)getCost(costs, n - nx_);
207  float de = INVSQRT2 * (float)getCost(costs, n + nx_);
208  potential[n] = pot;
209  //ROS_INFO("UPDATE %d %d %d %f", n, n%nx, n/nx, potential[n]);
210  if (pot < threshold_) // low-cost buffer block
211  {
212  if (potential[n - 1] > pot + le)
213  push_next(n-1);
214  if (potential[n + 1] > pot + re)
215  push_next(n+1);
216  if (potential[n - nx_] > pot + ue)
217  push_next(n-nx_);
218  if (potential[n + nx_] > pot + de)
219  push_next(n+nx_);
220  } else // overflow block
221  {
222  if (potential[n - 1] > pot + le)
223  push_over(n-1);
224  if (potential[n + 1] > pot + re)
225  push_over(n+1);
226  if (potential[n - nx_] > pot + ue)
227  push_over(n-nx_);
228  if (potential[n + nx_] > pot + de)
229  push_over(n+nx_);
230  }
231  }
232 }
233 
234 } //end namespace global_planner
#define push_over(n)
Definition: dijkstra.h:53
#define push_next(n)
Definition: dijkstra.h:52
virtual float calculatePotential(float *potential, unsigned char cost, int n, float prev_potential=-1)
#define INVSQRT2
Definition: dijkstra.cpp:190
DijkstraExpansion(PotentialCalculator *p_calc, int nx, int ny)
Definition: dijkstra.cpp:42
virtual void setSize(int nx, int ny)
Sets or resets the size of the map.
Definition: expander.h:59
void setSize(int nx, int ny)
Sets or resets the size of the map.
Definition: dijkstra.cpp:63
unsigned char lethal_cost_
Definition: expander.h:98
void updateCell(unsigned char *costs, float *potential, int n)
Updates the cell at index n.
Definition: dijkstra.cpp:192
#define POT_HIGH
Definition: planner_core.h:40
int toIndex(int x, int y)
Definition: expander.h:92
float getCost(unsigned char *costs, int n)
Definition: dijkstra.h:86
unsigned char neutral_cost_
Definition: expander.h:98
#define PRIORITYBUFSIZE
Definition: dijkstra.h:41
PotentialCalculator * p_calc_
Definition: expander.h:101
bool calculatePotentials(unsigned char *costs, double start_x, double start_y, double end_x, double end_y, int cycles, float *potential)
Definition: dijkstra.cpp:80
#define push_cur(n)
Definition: dijkstra.h:51


global_planner
Author(s): David Lu!!
autogenerated on Thu Mar 12 2020 04:10:23