gradient_path.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/gradient_path.h>
00039 #include <algorithm>
00040 #include <stdio.h>
00041 #include <global_planner/planner_core.h>
00042 
00043 namespace global_planner {
00044 
00045 GradientPath::GradientPath(PotentialCalculator* p_calc) :
00046         Traceback(p_calc), pathStep_(0.5) {
00047     gradx_ = grady_ = NULL;
00048 }
00049 
00050 GradientPath::~GradientPath() {
00051 
00052     if (gradx_)
00053         delete[] gradx_;
00054     if (grady_)
00055         delete[] grady_;
00056 }
00057 
00058 void GradientPath::setSize(int xs, int ys) {
00059     Traceback::setSize(xs, ys);
00060     if (gradx_)
00061         delete[] gradx_;
00062     if (grady_)
00063         delete[] grady_;
00064     gradx_ = new float[xs * ys];
00065     grady_ = new float[xs * ys];
00066 }
00067 
00068 bool GradientPath::getPath(float* potential, double start_x, double start_y, double goal_x, double goal_y, std::vector<std::pair<float, float> >& path) {
00069     std::pair<float, float> current;
00070     int stc = getIndex(goal_x, goal_y);
00071 
00072     // set up offset
00073     float dx = goal_x - (int)goal_x;
00074     float dy = goal_y - (int)goal_y;
00075     int ns = xs_ * ys_;
00076     memset(gradx_, 0, ns * sizeof(float));
00077     memset(grady_, 0, ns * sizeof(float));
00078 
00079     int c = 0;
00080     while (c++<ns*4) {
00081         // check if near goal
00082         double nx = stc % xs_ + dx, ny = stc / xs_ + dy;
00083 
00084         if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .5) {
00085             current.first = start_x;
00086             current.second = start_y;
00087             path.push_back(current);
00088             return true;
00089         }
00090 
00091         if (stc < xs_ || stc > xs_ * ys_ - xs_) // would be out of bounds
00092         {
00093             printf("[PathCalc] Out of bounds\n");
00094             return false;
00095         }
00096 
00097         current.first = nx;
00098         current.second = ny;
00099 
00100         //ROS_INFO("%d %d | %f %f ", stc%xs_, stc/xs_, dx, dy);
00101 
00102         path.push_back(current);
00103 
00104         bool oscillation_detected = false;
00105         int npath = path.size();
00106         if (npath > 2 && path[npath - 1].first == path[npath - 3].first
00107                 && path[npath - 1].second == path[npath - 3].second) {
00108             ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
00109             oscillation_detected = true;
00110         }
00111 
00112         int stcnx = stc + xs_;
00113         int stcpx = stc - xs_;
00114 
00115         // check for potentials at eight positions near cell
00116         if (potential[stc] >= POT_HIGH || potential[stc + 1] >= POT_HIGH || potential[stc - 1] >= POT_HIGH
00117                 || potential[stcnx] >= POT_HIGH || potential[stcnx + 1] >= POT_HIGH || potential[stcnx - 1] >= POT_HIGH
00118                 || potential[stcpx] >= POT_HIGH || potential[stcpx + 1] >= POT_HIGH || potential[stcpx - 1] >= POT_HIGH
00119                 || oscillation_detected) {
00120             ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potential[stc], (int) path.size());
00121             // check eight neighbors to find the lowest
00122             int minc = stc;
00123             int minp = potential[stc];
00124             int st = stcpx - 1;
00125             if (potential[st] < minp) {
00126                 minp = potential[st];
00127                 minc = st;
00128             }
00129             st++;
00130             if (potential[st] < minp) {
00131                 minp = potential[st];
00132                 minc = st;
00133             }
00134             st++;
00135             if (potential[st] < minp) {
00136                 minp = potential[st];
00137                 minc = st;
00138             }
00139             st = stc - 1;
00140             if (potential[st] < minp) {
00141                 minp = potential[st];
00142                 minc = st;
00143             }
00144             st = stc + 1;
00145             if (potential[st] < minp) {
00146                 minp = potential[st];
00147                 minc = st;
00148             }
00149             st = stcnx - 1;
00150             if (potential[st] < minp) {
00151                 minp = potential[st];
00152                 minc = st;
00153             }
00154             st++;
00155             if (potential[st] < minp) {
00156                 minp = potential[st];
00157                 minc = st;
00158             }
00159             st++;
00160             if (potential[st] < minp) {
00161                 minp = potential[st];
00162                 minc = st;
00163             }
00164             stc = minc;
00165             dx = 0;
00166             dy = 0;
00167 
00168             //ROS_DEBUG("[Path] Pot: %0.1f  pos: %0.1f,%0.1f",
00169             //    potential[stc], path[npath-1].first, path[npath-1].second);
00170 
00171             if (potential[stc] >= POT_HIGH) {
00172                 ROS_DEBUG("[PathCalc] No path found, high potential");
00173                 //savemap("navfn_highpot");
00174                 return 0;
00175             }
00176         }
00177 
00178         // have a good gradient here
00179         else {
00180 
00181             // get grad at four positions near cell
00182             gradCell(potential, stc);
00183             gradCell(potential, stc + 1);
00184             gradCell(potential, stcnx);
00185             gradCell(potential, stcnx + 1);
00186 
00187             // get interpolated gradient
00188             float x1 = (1.0 - dx) * gradx_[stc] + dx * gradx_[stc + 1];
00189             float x2 = (1.0 - dx) * gradx_[stcnx] + dx * gradx_[stcnx + 1];
00190             float x = (1.0 - dy) * x1 + dy * x2; // interpolated x
00191             float y1 = (1.0 - dx) * grady_[stc] + dx * grady_[stc + 1];
00192             float y2 = (1.0 - dx) * grady_[stcnx] + dx * grady_[stcnx + 1];
00193             float y = (1.0 - dy) * y1 + dy * y2; // interpolated y
00194 
00195             // show gradients
00196             ROS_DEBUG(
00197                     "[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);
00198 
00199             // check for zero gradient, failed
00200             if (x == 0.0 && y == 0.0) {
00201                 ROS_DEBUG("[PathCalc] Zero gradient");
00202                 return 0;
00203             }
00204 
00205             // move in the right direction
00206             float ss = pathStep_ / hypot(x, y);
00207             dx += x * ss;
00208             dy += y * ss;
00209 
00210             // check for overflow
00211             if (dx > 1.0) {
00212                 stc++;
00213                 dx -= 1.0;
00214             }
00215             if (dx < -1.0) {
00216                 stc--;
00217                 dx += 1.0;
00218             }
00219             if (dy > 1.0) {
00220                 stc += xs_;
00221                 dy -= 1.0;
00222             }
00223             if (dy < -1.0) {
00224                 stc -= xs_;
00225                 dy += 1.0;
00226             }
00227 
00228         }
00229 
00230         //printf("[Path] Pot: %0.1f  grad: %0.1f,%0.1f  pos: %0.1f,%0.1f\n",
00231         //         potential[stc], dx, dy, path[npath-1].first, path[npath-1].second);
00232     }
00233 
00234     return false;
00235 }
00236 
00237 /*
00238  int
00239  NavFn::calcPath(int n, int *st)
00240  {
00241  // set up start position at cell
00242  // st is always upper left corner for 4-point bilinear interpolation
00243  if (st == NULL) st = start;
00244  int stc = st[1]*nx + st[0];
00245 
00246  // go for <n> cycles at most
00247  for (int i=0; i<n; i++)
00248  {
00249 
00250 
00251 
00252  }
00253 
00254  //  return npath;            // out of cycles, return failure
00255  ROS_DEBUG("[PathCalc] No path found, path too long");
00256  //savemap("navfn_pathlong");
00257  return 0;            // out of cycles, return failure
00258  }
00259  */
00260 
00261 //
00262 // gradient calculations
00263 //
00264 // calculate gradient at a cell
00265 // positive value are to the right and down
00266 float GradientPath::gradCell(float* potential, int n) {
00267     if (gradx_[n] + grady_[n] > 0.0)    // check this cell
00268         return 1.0;
00269 
00270     if (n < xs_ || n > xs_ * ys_ - xs_)    // would be out of bounds
00271         return 0.0;
00272     float cv = potential[n];
00273     float dx = 0.0;
00274     float dy = 0.0;
00275 
00276     // check for in an obstacle
00277     if (cv >= POT_HIGH) {
00278         if (potential[n - 1] < POT_HIGH)
00279             dx = -lethal_cost_;
00280         else if (potential[n + 1] < POT_HIGH)
00281             dx = lethal_cost_;
00282 
00283         if (potential[n - xs_] < POT_HIGH)
00284             dy = -lethal_cost_;
00285         else if (potential[xs_ + 1] < POT_HIGH)
00286             dy = lethal_cost_;
00287     }
00288 
00289     else                // not in an obstacle
00290     {
00291         // dx calc, average to sides
00292         if (potential[n - 1] < POT_HIGH)
00293             dx += potential[n - 1] - cv;
00294         if (potential[n + 1] < POT_HIGH)
00295             dx += cv - potential[n + 1];
00296 
00297         // dy calc, average to sides
00298         if (potential[n - xs_] < POT_HIGH)
00299             dy += potential[n - xs_] - cv;
00300         if (potential[n + xs_] < POT_HIGH)
00301             dy += cv - potential[n + xs_];
00302     }
00303 
00304     // normalize
00305     float norm = hypot(dx, dy);
00306     if (norm > 0) {
00307         norm = 1.0 / norm;
00308         gradx_[n] = norm * dx;
00309         grady_[n] = norm * dy;
00310     }
00311     return norm;
00312 }
00313 
00314 } //end namespace global_planner
00315 


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