std::pair<std::vector<StateOfCar>, Seed> AStarSeed::findPathToTargetWithAstar(const cv::Mat& img,const State& start,const State& goal) {
// USE : for garanteed termination of planner
int no_of_iterations = 0;
fusionMap = img;
MAP_MAX_ROWS = img.rows;
MAP_MAX_COLS = img.cols;
if(DT==1)
distanceTransform();
if (DEBUG) {
image = fusionMap.clone();
}
StateOfCar startState(start), targetState(goal);
std::map<StateOfCar, open_map_element> openMap;
std::map<StateOfCar,StateOfCar, comparatorMapState> came_from;
SS::PriorityQueue<StateOfCar> openSet;
openSet.push(startState);
if (startState.isCloseTo(targetState)) {
std::cout<<"Bot is On Target"<<std::endl;
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
else if(isOnTheObstacle(startState)){
std::cout<<"Bot is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
else if(isOnTheObstacle(targetState)){
std::cout<<"Target is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
while (!openSet.empty() && ros::ok()) {
// std::cout<<"openSet size : "<<openSet.size()<<"\n";
if(no_of_iterations > MAX_ITERATIONS){
std::cerr<<"Overflow : openlist size : "<<openSet.size()<<"\n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
StateOfCar currentState=openSet.top();
if (openMap.find(currentState)!=openMap.end() && openMap[currentState].membership == CLOSED) {
openSet.pop();
}
currentState=openSet.top();
if (DEBUG) {
std::cout<<"current x : "<<currentState.x()<<" current y : "<<currentState.y()<<std::endl;
plotPointInMap(currentState);
cv::imshow("[PLANNER] Map", image);
cvWaitKey(0);
}
// TODO : use closeTo instead of onTarget
if (currentState.isCloseTo(targetState)) {
std::cout<<"openSet size : "<<openSet.size()<<"\n";
// std::cout<<"Target Reached"<<std::endl;
return reconstructPath(currentState, came_from);
}
openSet.pop();
openMap[currentState].membership = UNASSIGNED;
openMap[currentState].cost=-currentState.gCost();
openMap[currentState].membership=CLOSED;
std::vector<StateOfCar> neighborsOfCurrentState = neighborNodesWithSeeds(currentState);
for (unsigned int iterator=0; iterator < neighborsOfCurrentState.size(); iterator++) {
StateOfCar neighbor = neighborsOfCurrentState[iterator];
double tentativeGCostAlongFollowedPath = neighbor.gCost() + currentState.gCost();
double admissible = neighbor.distanceTo(targetState);
double consistent = admissible;
double intensity = fusionMap.at<uchar>(neighbor.y(), neighbor.x());
double consistentAndIntensity = (neighbor.hCost()*neighbor.hCost()+2 + intensity*intensity)/(neighbor.hCost()+intensity+2);
if (!((openMap.find(neighbor) != openMap.end()) &&
(openMap[neighbor].membership == OPEN))) {
came_from[neighbor] = currentState;
neighbor.gCost( tentativeGCostAlongFollowedPath) ;
if(DT==1)
neighbor.hCost(consistentAndIntensity);
else
neighbor.hCost( consistent) ;
neighbor.updateTotalCost();
openSet.push(neighbor);
openMap[neighbor].membership = OPEN;
openMap[neighbor].cost = neighbor.gCost();
}
}
no_of_iterations++;
}
std::cerr<<"NO PATH FOUND"<<std::endl;
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
std::pair <std::vector<StateOfCar>, Seed> AStarSeed::findPathToTarget(const cv::Mat& map, const State& start, const State& goal, const int distance_transform, const int debug_current_state, int& status) {
// USE : for guaranteed termination of planner
int no_of_iterations = 0;
int max_iterations = 10000;
node_handle.getParam("local_planner/max_iterations", max_iterations);
fusion_map = map;
map_max_rows = map.rows;
map_max_cols = map.cols;
if (distance_transform == 1) {
distanceTransform();
}
if (debug_current_state) {
image = fusion_map.clone();
}
StateOfCar start_state(start), target_state(goal);
std::map<StateOfCar, open_map_element> open_map;
std::map<StateOfCar, StateOfCar, comparatorMapState> came_from;
SS::PriorityQueue<StateOfCar> open_set;
open_set.push(start_state);
if (start_state.isCloseTo(target_state)) {
status = 1;
return std::make_pair(std::vector<StateOfCar>(), Seed());
} else if (isOnTheObstacle(start_state)) {
std::cout << "Bot is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
} else if (isOnTheObstacle(target_state)) {
std::cout << "Target is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
while (!open_set.empty() && ros::ok()) {
if (no_of_iterations > max_iterations) {
status = open_set.size();
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
StateOfCar current_state = open_set.top();
if (open_map.find(current_state) != open_map.end() && open_map[current_state].membership == CLOSED) {
open_set.pop();
}
current_state = open_set.top();
if (debug_current_state) {
std::cout << "current x : " << current_state.x() << " current y : " << current_state.y() << std::endl;
plotPointInMap(current_state);
cv::imshow("[PLANNER] Map", image);
cvWaitKey(0);
}
// TODO : use closeTo instead of onTarget
if (current_state.isCloseTo(target_state)) {
status = 2;
return reconstructPath(current_state, came_from);
}
open_set.pop();
open_map[current_state].membership = UNASSIGNED;
open_map[current_state].cost = -current_state.gCost();
open_map[current_state].membership = CLOSED;
std::vector<StateOfCar> neighbors_of_current_state = neighborNodesWithSeeds(current_state);
for (unsigned int iterator = 0; iterator < neighbors_of_current_state.size(); iterator++) {
StateOfCar neighbor = neighbors_of_current_state[iterator];
double tentative_gcost_along_followed_path = neighbor.gCost() + current_state.gCost();
double admissible = neighbor.distanceTo(target_state);
double consistent = admissible;
if (!((open_map.find(neighbor) != open_map.end()) &&
(open_map[neighbor].membership == OPEN))) {
came_from[neighbor] = current_state;
neighbor.hCost(consistent);
neighbor.gCost(tentative_gcost_along_followed_path);
neighbor.updateTotalCost();
open_set.push(neighbor);
open_map[neighbor].membership = OPEN;
open_map[neighbor].cost = neighbor.gCost();
}
}
no_of_iterations++;
}
status = 0;
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
