bool GradientPath::getPath (float* potential, double start_x, double start_y, double goal_x, double goal_y, std::vector<std::pair<float, float> >& path)
{
std::pair<float, float> current;
int stc = getIndex (goal_x, goal_y);
// set up offset
float dx = goal_x - (int) goal_x;
float dy = goal_y - (int) goal_y;
int ns = xs_ * ys_;
memset (gradx_, 0, ns * sizeof (float));
memset (grady_, 0, ns * sizeof (float));
int c = 0;
while (c++ < ns * 4)
{
// check if near goal
double nx = stc % xs_ + dx, ny = stc / xs_ + dy;
if (fabs (nx - start_x) < .5 && fabs (ny - start_y) < .5)
{
current.first = start_x;
current.second = start_y;
path.push_back (current);
return true;
}
if (stc < xs_ || stc > xs_ * ys_ - xs_) // would be out of bounds
{
printf ("[PathCalc] Out of bounds\n");
return false;
}
current.first = nx;
current.second = ny;
//ROS_INFO("%d %d | %f %f ", stc%xs_, stc/xs_, dx, dy);
path.push_back (current);
bool oscillation_detected = false;
int npath = path.size();
if (npath > 2 && path[npath - 1].first == path[npath - 3].first
&& path[npath - 1].second == path[npath - 3].second)
{
ROS_DEBUG ("[PathCalc] oscillation detected, attempting fix.");
oscillation_detected = true;
}
int stcnx = stc + xs_;
int stcpx = stc - xs_;
// check for potentials at eight positions near cell
if (potential[stc] >= POT_HIGH || potential[stc + 1] >= POT_HIGH || potential[stc - 1] >= POT_HIGH
|| potential[stcnx] >= POT_HIGH || potential[stcnx + 1] >= POT_HIGH || potential[stcnx - 1] >= POT_HIGH
|| potential[stcpx] >= POT_HIGH || potential[stcpx + 1] >= POT_HIGH || potential[stcpx - 1] >= POT_HIGH
|| oscillation_detected)
{
ROS_DEBUG ("[Path] Pot fn boundary, following grid (%0.1f/%d)", potential[stc], (int) path.size());
// check eight neighbors to find the lowest
int minc = stc;
int minp = potential[stc];
int st = stcpx - 1;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st = stc - 1;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st = stc + 1;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st = stcnx - 1;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp)
{
minp = potential[st];
minc = st;
}
stc = minc;
dx = 0;
dy = 0;
//ROS_DEBUG("[Path] Pot: %0.1f pos: %0.1f,%0.1f",
// potential[stc], path[npath-1].first, path[npath-1].second);
if (potential[stc] >= POT_HIGH)
{
ROS_DEBUG ("[PathCalc] No path found, high potential");
//savemap("navfn_highpot");
return 0;
}
}
// have a good gradient here
else
{
// get grad at four positions near cell
gradCell (potential, stc);
gradCell (potential, stc + 1);
gradCell (potential, stcnx);
gradCell (potential, stcnx + 1);
// get interpolated gradient
float x1 = (1.0 - dx) * gradx_[stc] + dx * gradx_[stc + 1];
float x2 = (1.0 - dx) * gradx_[stcnx] + dx * gradx_[stcnx + 1];
float x = (1.0 - dy) * x1 + dy * x2; // interpolated x
float y1 = (1.0 - dx) * grady_[stc] + dx * grady_[stc + 1];
float y2 = (1.0 - dx) * grady_[stcnx] + dx * grady_[stcnx + 1];
float y = (1.0 - dy) * y1 + dy * y2; // interpolated y
// show gradients
ROS_DEBUG (
"[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);
// check for zero gradient, failed
if (x == 0.0 && y == 0.0)
{
ROS_DEBUG ("[PathCalc] Zero gradient");
return 0;
}
// move in the right direction
float ss = pathStep_ / hypot (x, y);
dx += x * ss;
dy += y * ss;
// check for overflow
if (dx > 1.0)
{
stc++;
dx -= 1.0;
}
if (dx < -1.0)
{
stc--;
dx += 1.0;
}
if (dy > 1.0)
{
stc += xs_;
dy -= 1.0;
}
if (dy < -1.0)
{
stc -= xs_;
dy += 1.0;
}
}
//printf("[Path] Pot: %0.1f grad: %0.1f,%0.1f pos: %0.1f,%0.1f\n",
// potential[stc], dx, dy, path[npath-1].first, path[npath-1].second);
}
return false;
}
int
NavFn::calcPath(int n, int *st)
{
// test write
//savemap("test");
// check path arrays
if (npathbuf < n)
{
if (pathx) delete [] pathx;
if (pathy) delete [] pathy;
pathx = new float[n];
pathy = new float[n];
npathbuf = n;
}
// set up start position at cell
// st is always upper left corner for 4-point bilinear interpolation
if (st == NULL) st = start;
int stc = st[1]*nx + st[0];
// set up offset
float dx=0;
float dy=0;
npath = 0;
// go for <n> cycles at most
for (int i=0; i<n; i++)
{
// check if near goal
int nearest_point=std::max(0,std::min(nx*ny-1,stc+(int)round(dx)+(int)(nx*round(dy))));
if (potarr[nearest_point] < COST_NEUTRAL)
{
pathx[npath] = (float)goal[0];
pathy[npath] = (float)goal[1];
return ++npath; // done!
}
if (stc < nx || stc > ns-nx) // would be out of bounds
{
ROS_DEBUG("[PathCalc] Out of bounds");
return 0;
}
// add to path
pathx[npath] = stc%nx + dx;
pathy[npath] = stc/nx + dy;
npath++;
bool oscillation_detected = false;
if( npath > 2 &&
pathx[npath-1] == pathx[npath-3] &&
pathy[npath-1] == pathy[npath-3] )
{
ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
oscillation_detected = true;
}
int stcnx = stc+nx;
int stcpx = stc-nx;
// check for potentials at eight positions near cell
if (potarr[stc] >= POT_HIGH ||
potarr[stc+1] >= POT_HIGH ||
potarr[stc-1] >= POT_HIGH ||
potarr[stcnx] >= POT_HIGH ||
potarr[stcnx+1] >= POT_HIGH ||
potarr[stcnx-1] >= POT_HIGH ||
potarr[stcpx] >= POT_HIGH ||
potarr[stcpx+1] >= POT_HIGH ||
potarr[stcpx-1] >= POT_HIGH ||
oscillation_detected)
{
ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potarr[stc], npath);
// check eight neighbors to find the lowest
int minc = stc;
int minp = potarr[stc];
int st = stcpx - 1;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st++;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st++;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st = stc-1;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st = stc+1;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st = stcnx-1;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st++;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st++;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
stc = minc;
dx = 0;
dy = 0;
ROS_DEBUG("[Path] Pot: %0.1f pos: %0.1f,%0.1f",
potarr[stc], pathx[npath-1], pathy[npath-1]);
if (potarr[stc] >= POT_HIGH)
{
ROS_DEBUG("[PathCalc] No path found, high potential");
//savemap("navfn_highpot");
return 0;
}
}
// have a good gradient here
else
{
// get grad at four positions near cell
gradCell(stc);
gradCell(stc+1);
gradCell(stcnx);
gradCell(stcnx+1);
// get interpolated gradient
float x1 = (1.0-dx)*gradx[stc] + dx*gradx[stc+1];
float x2 = (1.0-dx)*gradx[stcnx] + dx*gradx[stcnx+1];
float x = (1.0-dy)*x1 + dy*x2; // interpolated x
float y1 = (1.0-dx)*grady[stc] + dx*grady[stc+1];
float y2 = (1.0-dx)*grady[stcnx] + dx*grady[stcnx+1];
float y = (1.0-dy)*y1 + dy*y2; // interpolated y
// show gradients
ROS_DEBUG("[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);
// check for zero gradient, failed
if (x == 0.0 && y == 0.0)
{
ROS_DEBUG("[PathCalc] Zero gradient");
return 0;
}
// move in the right direction
float ss = pathStep/sqrt(x*x+y*y);
dx += x*ss;
dy += y*ss;
// check for overflow
if (dx > 1.0) { stc++; dx -= 1.0; }
if (dx < -1.0) { stc--; dx += 1.0; }
if (dy > 1.0) { stc+=nx; dy -= 1.0; }
if (dy < -1.0) { stc-=nx; dy += 1.0; }
}
// ROS_INFO("[Path] Pot: %0.1f grad: %0.1f,%0.1f pos: %0.1f,%0.1f\n",
// potarr[stc], x, y, pathx[npath-1], pathy[npath-1]);
}
// return npath; // out of cycles, return failure
ROS_DEBUG("[PathCalc] No path found, path too long");
//savemap("navfn_pathlong");
return 0; // out of cycles, return failure
}
