dijkstra.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, 2013, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * Author: Eitan Marder-Eppstein
00036  *         David V. Lu!!
00037  *********************************************************************/
00038 #include<global_planner/dijkstra.h>
00039 #include <algorithm>
00040 namespace global_planner {
00041 
00042 DijkstraExpansion::DijkstraExpansion(PotentialCalculator* p_calc, int nx, int ny) :
00043         Expander(p_calc, nx, ny), pending_(NULL), precise_(false) {
00044     // priority buffers
00045     buffer1_ = new int[PRIORITYBUFSIZE];
00046     buffer2_ = new int[PRIORITYBUFSIZE];
00047     buffer3_ = new int[PRIORITYBUFSIZE];
00048 
00049     priorityIncrement_ = 2 * neutral_cost_;
00050 }
00051 
00052 DijkstraExpansion::~DijkstraExpansion() {
00053   delete[] buffer1_;
00054   delete[] buffer2_;
00055   delete[] buffer3_;
00056   if (pending_)
00057       delete[] pending_;
00058 }
00059 
00060 //
00061 // Set/Reset map size
00062 //
00063 void DijkstraExpansion::setSize(int xs, int ys) {
00064     Expander::setSize(xs, ys);
00065     if (pending_)
00066         delete[] pending_;
00067 
00068     pending_ = new bool[ns_];
00069     memset(pending_, 0, ns_ * sizeof(bool));
00070 }
00071 
00072 //
00073 // main propagation function
00074 // Dijkstra method, breadth-first
00075 // runs for a specified number of cycles,
00076 //   or until it runs out of cells to update,
00077 //   or until the Start cell is found (atStart = true)
00078 //
00079 
00080 bool DijkstraExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
00081                                            int cycles, float* potential) {
00082     cells_visited_ = 0;
00083     // priority buffers
00084     threshold_ = lethal_cost_;
00085     currentBuffer_ = buffer1_;
00086     currentEnd_ = 0;
00087     nextBuffer_ = buffer2_;
00088     nextEnd_ = 0;
00089     overBuffer_ = buffer3_;
00090     overEnd_ = 0;
00091     memset(pending_, 0, ns_ * sizeof(bool));
00092     std::fill(potential, potential + ns_, POT_HIGH);
00093 
00094     // set goal
00095     int k = toIndex(start_x, start_y);
00096 
00097     if(precise_)
00098     {
00099         double dx = start_x - (int)start_x, dy = start_y - (int)start_y;
00100         dx = floorf(dx * 100 + 0.5) / 100;
00101         dy = floorf(dy * 100 + 0.5) / 100;
00102         potential[k] = neutral_cost_ * 2 * dx * dy;
00103         potential[k+1] = neutral_cost_ * 2 * (1-dx)*dy;
00104         potential[k+nx_] = neutral_cost_*2*dx*(1-dy);
00105         potential[k+nx_+1] = neutral_cost_*2*(1-dx)*(1-dy);//*/
00106 
00107         push_cur(k+2);
00108         push_cur(k-1);
00109         push_cur(k+nx_-1);
00110         push_cur(k+nx_+2);
00111 
00112         push_cur(k-nx_);
00113         push_cur(k-nx_+1);
00114         push_cur(k+nx_*2);
00115         push_cur(k+nx_*2+1);
00116     }else{
00117         potential[k] = 0;
00118         push_cur(k+1);
00119         push_cur(k-1);
00120         push_cur(k-nx_);
00121         push_cur(k+nx_);
00122     }
00123 
00124     int nwv = 0;            // max priority block size
00125     int nc = 0;            // number of cells put into priority blocks
00126     int cycle = 0;        // which cycle we're on
00127 
00128     // set up start cell
00129     int startCell = toIndex(end_x, end_y);
00130 
00131     for (; cycle < cycles; cycle++) // go for this many cycles, unless interrupted
00132             {
00133         // 
00134         if (currentEnd_ == 0 && nextEnd_ == 0) // priority blocks empty
00135             return false;
00136 
00137         // stats
00138         nc += currentEnd_;
00139         if (currentEnd_ > nwv)
00140             nwv = currentEnd_;
00141 
00142         // reset pending_ flags on current priority buffer
00143         int *pb = currentBuffer_;
00144         int i = currentEnd_;
00145         while (i-- > 0)
00146             pending_[*(pb++)] = false;
00147 
00148         // process current priority buffer
00149         pb = currentBuffer_;
00150         i = currentEnd_;
00151         while (i-- > 0)
00152             updateCell(costs, potential, *pb++);
00153 
00154         // swap priority blocks currentBuffer_ <=> nextBuffer_
00155         currentEnd_ = nextEnd_;
00156         nextEnd_ = 0;
00157         pb = currentBuffer_;        // swap buffers
00158         currentBuffer_ = nextBuffer_;
00159         nextBuffer_ = pb;
00160 
00161         // see if we're done with this priority level
00162         if (currentEnd_ == 0) {
00163             threshold_ += priorityIncrement_;    // increment priority threshold
00164             currentEnd_ = overEnd_;    // set current to overflow block
00165             overEnd_ = 0;
00166             pb = currentBuffer_;        // swap buffers
00167             currentBuffer_ = overBuffer_;
00168             overBuffer_ = pb;
00169         }
00170 
00171         // check if we've hit the Start cell
00172         if (potential[startCell] < POT_HIGH)
00173             break;
00174     }
00175     //ROS_INFO("CYCLES %d/%d ", cycle, cycles);
00176     if (cycle < cycles)
00177         return true; // finished up here
00178     else
00179         return false;
00180 }
00181 
00182 //
00183 // Critical function: calculate updated potential value of a cell,
00184 //   given its neighbors' values
00185 // Planar-wave update calculation from two lowest neighbors in a 4-grid
00186 // Quadratic approximation to the interpolated value
00187 // No checking of bounds here, this function should be fast
00188 //
00189 
00190 #define INVSQRT2 0.707106781
00191 
00192 inline void DijkstraExpansion::updateCell(unsigned char* costs, float* potential, int n) {
00193     cells_visited_++;
00194 
00195     // do planar wave update
00196     float c = getCost(costs, n);
00197     if (c >= lethal_cost_)    // don't propagate into obstacles
00198         return;
00199 
00200     float pot = p_calc_->calculatePotential(potential, c, n);
00201 
00202     // now add affected neighbors to priority blocks
00203     if (pot < potential[n]) {
00204         float le = INVSQRT2 * (float)getCost(costs, n - 1);
00205         float re = INVSQRT2 * (float)getCost(costs, n + 1);
00206         float ue = INVSQRT2 * (float)getCost(costs, n - nx_);
00207         float de = INVSQRT2 * (float)getCost(costs, n + nx_);
00208         potential[n] = pot;
00209         //ROS_INFO("UPDATE %d %d %d %f", n, n%nx, n/nx, potential[n]);
00210         if (pot < threshold_)    // low-cost buffer block
00211                 {
00212             if (potential[n - 1] > pot + le)
00213                 push_next(n-1);
00214             if (potential[n + 1] > pot + re)
00215                 push_next(n+1);
00216             if (potential[n - nx_] > pot + ue)
00217                 push_next(n-nx_);
00218             if (potential[n + nx_] > pot + de)
00219                 push_next(n+nx_);
00220         } else            // overflow block
00221         {
00222             if (potential[n - 1] > pot + le)
00223                 push_over(n-1);
00224             if (potential[n + 1] > pot + re)
00225                 push_over(n+1);
00226             if (potential[n - nx_] > pot + ue)
00227                 push_over(n-nx_);
00228             if (potential[n + nx_] > pot + de)
00229                 push_over(n+nx_);
00230         }
00231     }
00232 }
00233 
00234 } //end namespace global_planner


global_planner
Author(s): David Lu!!
autogenerated on Wed Aug 2 2017 03:13:12