bool AStarSeed::isWalkableWithSeeds(StateOfCar const& startState, StateOfCar const& targetState) { bool NoObstacle = true; for (std::vector<State>::iterator stateIt = givenSeeds[targetState.seedTaken()].intermediatePoints.begin(); stateIt != givenSeeds[targetState.seedTaken()].intermediatePoints.end(); ++stateIt) { const State state= *stateIt; double alpha = startState.theta(); int x = state.x(); int y = state.y(); int intermediateXcordinate = (int) (x * sin(alpha * (CV_PI / 180)) + y * cos(alpha * (CV_PI / 180)) + startState.x()); int intermediateYcordinate = (int) (-x * cos(alpha * (CV_PI / 180)) + y * sin(alpha * (CV_PI / 180)) + startState.y()); if (((intermediateXcordinate >= 0) && (intermediateXcordinate < MAP_MAX)) && (( intermediateYcordinate >= 0) && (intermediateYcordinate < MAP_MAX))) { fusionMap.at<uchar>(fusionMap.rows - intermediateYcordinate -1, intermediateXcordinate) < 250 ? NoObstacle *= 1 : NoObstacle *= 0; } else { return false; } } if(fusionMap.at<uchar>(fusionMap.rows - targetState.y() -1, targetState.x()) >= 250) return false; return NoObstacle == true; }
inline bool operator<(const StateOfCar& b) const { double k11 = x(); double k12 = y(); double k13 = theta(); double cantor11 = 0.5 * (k11 + k12) * (k11 + k12 + 1) + k12; double cantor12 = 0.5 * (cantor11 + k13) * (cantor11 + k13 + 1) + k13; double k21 = b.x(); double k22 = b.y(); double k23 = b.theta(); double cantor21 = 0.5 * (k21 + k22) * (k21 + k22 + 1) + k22; double cantor22 = 0.5 * (cantor21 + k23) * (cantor21 + k23 + 1) + k23; return cantor12 < cantor22; // return f_cost_ > b.totalCost(); }
bool AStarSeed::onTarget(StateOfCar const& currentState, const StateOfCar& targetState) { for (unsigned int i = 0; i < givenSeeds.size(); i++) { for (unsigned int j = 0; j < givenSeeds[i].intermediatePoints.size(); j++) { const double sx = givenSeeds[i].intermediatePoints[j].x(); const double sy = givenSeeds[i].finalState.x(); //double sz = givenSeeds[i].destination.getXcordinate(); const int x ((int) (currentState.x() + sx * sin(currentState.theta() * (CV_PI / 180)) + sy * cos(currentState.theta() * (CV_PI / 180)))); const int y ((int) (currentState.y() -sx * cos(currentState.theta() * (CV_PI / 180)) + sy * sin(currentState.theta() * (CV_PI / 180)))); StateOfCar temp(State(x,y,0,0)); if (temp.isCloseTo(targetState)) { return true; } } } return false; }
inline bool operator()(const StateOfCar& a, const StateOfCar& b) { double k11 = a.x(); double k12 = a.y(); double k13 = a.theta(); double cantor11 = 0.5 * (k11 + k12) * (k11 + k12 + 1) + k12; double cantor12 = 0.5 * (cantor11 + k13) * (cantor11 + k13 + 1) + k13; double k21 = b.x(); double k22 = b.y(); double k23 = b.theta(); double cantor21 = 0.5 * (k21 + k22) * (k21 + k22 + 1) + k22; double cantor22 = 0.5 * (cantor21 + k23) * (cantor21 + k23 + 1) + k23; return cantor12 < cantor22; }
inline bool operator<(const StateOfCar& b) const { return f_cost_ > b.totalCost(); }
inline bool operator==(const StateOfCar& b) const { return x() == b.x() && y() == b.y(); }
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()); }