/* bool Dstar::replan()
* --------------------------
* Updates the costs for all cells and computes the shortest path to
* goal. Returns true if a path is found, false otherwise. The path is
* computed by doing a greedy search over the cost+g values in each
* cells. In order to get around the problem of the robot taking a
* path that is near a 45 degree angle to goal we break ties based on
* the metric euclidean(state, goal) + euclidean(state,start).
*/
bool Dstar::replan() {
path.clear();
int res = computeShortestPath();
//printf("res: %d ols: %d ohs: %d tk: [%f %f] sk: [%f %f] sgr: (%f,%f)\n",res,openList.size(),openHash.size(),openList.top().k.first,openList.top().k.second, s_start.k.first, s_start.k.second,getRHS(s_start),getG(s_start));
if (res < 0) {
fprintf(stderr, "NO PATH TO GOAL\n");
return false;
}
list<state> n;
list<state>::iterator i;
state cur = s_start;
if (isinf(getG(s_start))) {
fprintf(stderr, "NO PATH TO GOAL\n");
return false;
}
while(cur != s_goal) {
path.push_back(cur);
getSucc(cur, n);
if (n.empty()) {
fprintf(stderr, "NO PATH TO GOAL\n");
return false;
}
double cmin = INFINITY;
double tmin;
state smin;
for (i=n.begin(); i!=n.end(); i++) {
//if (occupied(*i)) continue;
double val = cost(cur,*i);
double val2 = trueDist(*i,s_goal) + trueDist(s_start,*i); // (Euclidean) cost to goal + cost to pred
val += getG(*i);
if (close(val,cmin)) {
if (tmin > val2) {
tmin = val2;
cmin = val;
smin = *i;
}
} else if (val < cmin) {
tmin = val2;
cmin = val;
smin = *i;
}
}
n.clear();
cur = smin;
}
path.push_back(s_goal);
return true;
}
/* bool Dstar::replan()
* --------------------------
* Updates the costs for all cells and computes the shortest path to
* goal. Returns true if a path is found, false otherwise. The path is
* computed by doing a greedy search over the cost+g values in each
* cells. In order to get around the problem of the robot taking a
* path that is near a 45 degree angle to goal we break ties based on
* the metric euclidean(state, goal) + euclidean(state,start).
*/
bool Dstar::replan() {
path.clear();
int res = computeShortestPath();
// printf("res: %d ols: %d ohs: %d tk: [%f %f] sk: [%f %f] sgr: (%f,%f)\n",res,openList.size(),openHash.size(),openList.top().k.first,openList.top().k.second, s_start.k.first, s_start.k.second,getRHS(s_start),getG(s_start));
if (res < 0) {
//fprintf(stderr, "NO PATH TO GOAL\n");
path.cost = INFINITY;
return false;
}
list<state> n;
list<state>::iterator i;
state cur = s_start;
state prev = s_start;
if (isinf(getG(s_start))) {
//fprintf(stderr, "NO PATH TO GOAL\n");
path.cost = INFINITY;
return false;
}
// constructs the path
while(cur != s_goal) {
path.path.push_back(cur);
path.cost += cost(prev,cur);
getSucc(cur, n);
if (n.empty()) {
//fprintf(stderr, "NO PATH TO GOAL\n");
path.cost = INFINITY;
return false;
}
// choose the next node in the path by selecting the one with smallest
// g() + cost. Break ties by choosing the neighbour that is closest
// to the line between start and goal (i.e. smallest sum of Euclidean
// distances to start and goal).
double cmin = INFINITY;
double tmin = INFINITY;
state smin = cur;
for (i=n.begin(); i!=n.end(); i++) {
if (occupied(*i)) continue;
double val = cost(cur,*i);
double val2 = trueDist(*i,s_goal) + trueDist(s_start,*i); // (Euclidean) cost to goal + cost to pred
double val3 = getG(*i);
val += val3;
// tiebreak if curent neighbour is equal to current best
// choose the neighbour that has the smallest tmin value
if (!isinf(val) && near(val,cmin)) {
if (val2 < tmin) {
tmin = val2;
cmin = val;
smin = *i;
}
}
// if next neighbour (*i) is scrictly lower cost than the
// current best, then set it to be the current best.
else if (val < cmin) {
tmin = val2;
cmin = val;
smin = *i;
}
} // end for loop
n.clear();
if( isinf(cmin) ) break;
prev = cur;
cur = smin;
} // end while loop
path.path.push_back(s_goal);
path.cost += cost(prev,s_goal);
return true;
}
