/* 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; }