int main(int argc, char** argv) { ros::init(argc, argv, "base_planner_cu"); ros::NodeHandle nh; ros::Rate loop_rate(100); // load globals from parameter server double param_input; bool bool_input; if(ros::param::get("base_planner_cu/obstacle_cost", param_input)) OBSTACLE_COST = param_input; // the cost of an obstacle if(ros::param::get("base_planner_cu/robot_radius", param_input)) robot_radius = param_input; //(m) if(ros::param::get("base_planner_cu/safety_distance", param_input)) safety_distance = param_input; //(m), distance that must be maintained between robot and obstacles if(ros::param::get("base_planner_cu/better_path_ratio", param_input)) old_path_discount = param_input; // if (current path length)*old_path_discount < (old path length) < (current path length)/old_path_discount, then stick with the old path if(ros::param::get("base_planner_cu/replan_jump", param_input)) MAX_POSE_JUMP = param_input; //(map grids) after which it is easer to replan from scratch if(ros::param::get("base_planner_cu/using_extra_safety_distance", bool_input)) high_cost_safety_region = bool_input; // true: dilate obstacles by an extra extra_dilation but instad of lethal, multiply existing cost by extra_dilation_mult if(ros::param::get("base_planner_cu/extra_safety_distance", param_input)) extra_dilation = param_input; //(m) // print data about parameters printf("obstacle cost: %f, robot radius: %f, safety_distance: %f, extra_safety_distance: %f, \nbetter_path_ratio: %f, replan_jump: %f \n", OBSTACLE_COST, robot_radius, safety_distance, extra_dilation, old_path_discount, MAX_POSE_JUMP); if(high_cost_safety_region) printf("using extra safety distance \n"); else printf("not using extra safety distance \n"); // wait until the map service is provided (we need its tf /world_cu -> /map_cu to be broadcast) ros::service::waitForService("/cu/get_map_cu", -1); // set up ROS topic subscriber callbacks pose_sub = nh.subscribe("/cu/pose_cu", 1, pose_callback); goal_sub = nh.subscribe("/cu/goal_cu", 1, goal_callback); map_changes_sub = nh.subscribe("/cu/map_changes_cu", 10, map_changes_callback); // set up ROS topic publishers global_path_pub = nh.advertise<nav_msgs::Path>("/cu/global_path_cu", 1); system_update_pub = nh.advertise<std_msgs::Int32>("/cu/system_update_cu", 10); // spin ros once ros::spinOnce(); loop_rate.sleep(); // wait for a map while(!load_map() && ros::ok()) { ros::spinOnce(); loop_rate.sleep(); } // init map buildGraphAndMap(raw_map->height, raw_map->width); populate_map_from_raw_map(); // wait until we have a goal and a robot pose (these arrive via callbacks) while((goal_pose == NULL || robot_pose == NULL) && ros::ok()) { ros::spinOnce(); loop_rate.sleep(); } // initialize search and related data structures (this calculates initial cost field) if(ros::ok()) { initialize_search(true); // true := use old map, (raw_map was populated in a pre-processing step) new_goal = false; ros::spinOnce(); // check in on callbacks // added to give map changes a chance to propogate before first real search ros::Rate one_time_sleep(2); one_time_sleep.sleep(); ros::spinOnce(); // check in on callbacks } // main planning loop int lr, ud; int time_counter = 0; MapPath* old_path = NULL; while (ros::ok()) { while(change_token_used) {printf("change token used, main \n");} change_token_used = true; time_counter++; //printf(" This is the base planner %d\n", time_counter); // heartbeat for debugging load_goal(); if(new_goal || reload_map) { if(reload_map) { printf("reinitializing map and searchtree \n"); change_token_used = false; // wait for a map while(!load_map() && ros::ok()) { ros::spinOnce(); loop_rate.sleep(); } change_token_used = true; initialize_search(false); // false := reset map based in what is in raw_map (raw_map was just re-populated with info from the map server) } else // new_goal { printf("reinitializing search tree, reusing map \n"); initialize_search(true); // true := reuse the old map } new_goal = false; reload_map = false; if(old_path != NULL) { deleteMapPath(old_path); old_path = NULL; } // get best estimate of pose while(!load_pose()) { // wait for most up to date pose } printf(" recieved pose via service \n"); } else load_pose(); // if services lag then this is a bad idea, but have had problems on netbooks never getting new pose // find the new exact coordinates of the robot robot_pos_x = robot_pose->x/raw_map->resolution; // need to add global offset ability if(robot_pos_x > raw_map->width) robot_pos_x = raw_map->width; if(robot_pos_x < 0) robot_pos_x = 0; robot_pos_y = robot_pose->y/raw_map->resolution; // need to add global offset ability if(robot_pos_y > raw_map->width) robot_pos_y = raw_map->width; if(robot_pos_y < 0) robot_pos_y = 0; // do replanning, note that we need to make sure that all neighboring nodes of the cell(s) containing the robot are updated //printf("replanning %d \n", time_counter); if(robot_pos_x == (double)((int)robot_pos_x)) // then on a vertical edge, need to plan to nodes on cells to the left and right of the robot lr = -1; else //only need to plan to nodes on cells with x == int_pos_x lr = 0; while(lr < 2) { if(robot_pos_y == (double)((int)robot_pos_y)) // then on a horizontal edge, need to plan to nodes on cells to the top and bottom of the robot ud = -1; else //only need to plan to nodes on cells with y == int_pos_y ud = 0; if((int)robot_pos_x + lr < 0 || (int)robot_pos_x + lr > WIDTH) { lr++; continue; } while(ud < 2) { if((int)robot_pos_y + ud < 0 || (int)robot_pos_y + ud > HEIGHT) { ud++; continue; } s_start = &graph[(int)robot_pos_y + ud][(int)robot_pos_x + lr]; computeShortestPath(); // this updates the cost field to this node ud++; } lr++; } // extract the path, this will be used to figure out where to move the robot //printf("extract path1 %d \n", time_counter); MapPath* pathToGoalWithLookAhead = extractPathOneLookahead(); double path1_max_single_grid; double path1_cost = calculatePathCost(pathToGoalWithLookAhead, path1_max_single_grid); //printf("extract path2 %d \n", time_counter); MapPath* pathToGoalMine = extractPathMine(0); // this uses gradient descent where possible double path2_max_single_grid; double path2_cost = calculatePathCost(pathToGoalMine, path2_max_single_grid); double old_path_cost = LARGE; double old_path_max_single_grid; if(old_path != NULL) old_path_cost = calculatePathCost(old_path, old_path_max_single_grid); change_token_used = false; // use better path of the two to move the robot (or retain the old path if it is still better) if(old_path != NULL && path1_cost*old_path_discount <= old_path_cost && old_path_cost <= path1_cost/old_path_discount && path2_cost*old_path_discount <= old_path_cost && old_path_cost <= path2_cost/old_path_discount && old_path_max_single_grid < OBSTACLE_COST) { publish_global_path(old_path); //printf("old path is about the same or better %d [%f vs %f] %f \n", time_counter, old_path_cost, (path1_cost+path2_cost)/2, old_path_max_single_grid); deleteMapPath(pathToGoalMine); deleteMapPath(pathToGoalWithLookAhead); } else if(path1_cost < path2_cost && path1_max_single_grid < OBSTACLE_COST) { publish_global_path(pathToGoalWithLookAhead); //printf("path1 is better %d %f %f\n", time_counter, path1_cost, path1_max_single_grid); if(old_path != NULL) { deleteMapPath(old_path); old_path = NULL; } old_path = pathToGoalWithLookAhead; deleteMapPath(pathToGoalMine); } else if(path2_max_single_grid < OBSTACLE_COST) { publish_global_path(pathToGoalMine); //printf("path2 is better %d %f %f\n", time_counter, path2_cost, path2_max_single_grid); if(old_path != NULL) { deleteMapPath(old_path); old_path = NULL; } old_path = pathToGoalMine; deleteMapPath(pathToGoalWithLookAhead); } else // all paths go through obstacles { printf("Base Planner: No safe path exists to goal %f %f %f\n", old_path_cost, path1_cost, path2_cost); publish_system_update(1); reload_map = true; } ros::spinOnce(); loop_rate.sleep(); //printf("done %d \n", time_counter); } if(old_path != NULL) deleteMapPath(old_path); pose_sub.shutdown(); goal_sub.shutdown(); map_changes_sub.shutdown(); global_path_pub.shutdown(); system_update_pub.shutdown(); destroy_pose(robot_pose); destroy_pose(goal_pose); cpDeleteHeap(primaryCellPathHeap); cpDeleteHeap(secondaryCellPathHeap); deleteHeap(); deleteGraphAndMap(); }
/* 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; }
// This sets up the global data structures to perform a new search from scratch. // It will clean up any old things that were part of a previous search. // This must be called after the map has changed size or the goal changes. // It assumes that raw_map, robot and goal pose all exist. // Returns true when successful // if reuse_map == true, then the map info is saved (saves on map dilation time, but only should be used for a goal change) bool initialize_search(bool reuse_map) { printf("Initializing Search to Goal, building search tree. \n"); // in case of re-initialization, clean up old values if(primaryCellPathHeap != NULL) cpDeleteHeap(primaryCellPathHeap); if(secondaryCellPathHeap != NULL) cpDeleteHeap(secondaryCellPathHeap); if(heapNode != NULL) deleteHeap(); if(graph != NULL) { if(reuse_map) deleteGraphAndMap(false); else deleteGraphAndMap(); } else reuse_map = false; // if graph is null, then map is most likely not allocated // init map and graph buildGraphAndMap(raw_map->height, raw_map->width); // load raw_map data into the new search map, dilating it by 1/2 robot rad + safety distance if(!reuse_map) populate_map_from_raw_map(); // remember start and goal locations double resolution = raw_map->resolution; int goalN = goal_pose->y/resolution; // need to add global offset ability if(goalN > (int)raw_map->height) goalN = (int)raw_map->height; if(goalN < 0) goalN = 0; int goalM = goal_pose->x/resolution; // need to add global offset ability if(goalM > (int)raw_map->width) goalM = (int)raw_map->width; if(goalM < 0) goalM = 0; int robotN = robot_pose->y/resolution; // need to add global offset ability if(robotN > (int)raw_map->height) robotN = (int)raw_map->height; if(robotN < 0) robotN = 0; int robotM = robot_pose->x/resolution; // need to add global offset ability if(robotM > (int)raw_map->width) robotM = (int)raw_map->width; if(robotM < 0) robotM = 0; // find the exact coordinates of the robot robot_pos_x = robot_pose->x/resolution; // need to add global offset ability if(robot_pos_x > raw_map->width) robot_pos_x = raw_map->width; if(robot_pos_x < 0) robot_pos_x = 0; robot_pos_y = robot_pose->y/resolution; // need to add global offset ability if(robot_pos_y > raw_map->width) robot_pos_y = raw_map->width; if(robot_pos_y < 0) robot_pos_y = 0; // init heap buildHeap(); // init search values s_start = &graph[robotN][robotM]; s_start->g = LARGE; s_start->rhs = LARGE; s_goal = &graph[goalN][goalM]; s_goal->g = LARGE; s_goal->rhs = 0; insert(s_goal,calculateKey(s_goal)); // compute the initial field costs computeShortestPath(); // create the cellPath search and look ahead heaps primaryCellPathHeap = cpBuildHeap(); secondaryCellPathHeap = cpBuildHeap(); 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; }
/* This method does the same thing as the pseudo-code's updateVertex(), except for grids instead of graphs. Pathfinding algorimths tend to be demonstraited with a graph rather than a grid, in order to update the cost between two tiles we must update both the tile and its neighbour. */ void GridWorld::updateCost(unsigned int x, unsigned int y, double newCost){ static int count = 1; count++; Tile* tile = getTileAt(x, y); printf("Updating <%d, %d> from %2.0lf to %2.0lf - Update: %d\n", x, y, tile->cost, newCost, count); km += calculateH(previous); previous = goal; //I am aware that the following code below could be refactored by 50% //since it's repeating itself with only a few changes double oldCost = tile->cost; double oldCostToTile, newCostToTile; //Update CURRENT by finding its new minimum RHS-value from NEIGHBOURS std::vector<Tile*> neighbours(getNeighbours(tile)); for (int i = 0; i < neighbours.size(); i++){ tile->cost = oldCost; oldCostToTile = calculateC(tile, neighbours[i]); tile->cost = newCost; newCostToTile = calculateC(tile, neighbours[i]); if (oldCostToTile > newCostToTile){ if (tile != start && tile->rhs > neighbours[i]->g + newCostToTile){ tile->successor = neighbours[i]; tile->rhs = neighbours[i]->g + newCostToTile; } } else if (tile != start && tile->successor == neighbours[i]){ TilePair minSucc(getMinSuccessor(tile)); tile->rhs = minSucc.second; tile->successor = (tile->rhs == PF_INFINITY ? 0 : minSucc.first); } } updateVertex(tile); //Update all NEIGHBOURING cells by finding their new min RHS-values from CURRENT for (int i = 0; i < neighbours.size(); i++){ tile->cost = oldCost; oldCostToTile = calculateC(tile, neighbours[i]); tile->cost = newCost; newCostToTile = calculateC(tile, neighbours[i]); if (oldCostToTile > newCostToTile){ if (neighbours[i] != start && neighbours[i]->rhs > tile->g + newCostToTile){ neighbours[i]->successor = tile; neighbours[i]->rhs = tile->g + newCostToTile; updateVertex(neighbours[i]); } } else if (neighbours[i] != start && neighbours[i]->successor == tile){ TilePair minSucc(getMinSuccessor(neighbours[i])); neighbours[i]->rhs = minSucc.second; neighbours[i]->successor = (neighbours[i]->rhs == PF_INFINITY ? 0 : minSucc.first); updateVertex(neighbours[i]); } } computeShortestPath(); }
/* This method does the same thing as the pseudo-code's updateVertex(), except for grids instead of graphs. Pathfinding algorimths tend to be demonstraited with a graph rather than a grid, in order to update the cost between two tiles we must update both the tile and its neighbour. */ void GridWorld::updateCost(unsigned int x, unsigned int y, double newCost){ static int count = 1; count++; Tile* tile = getTileAt(x, y); printf("Updating <%d, %d> from %2.0lf to %2.0lf - Update: %d\n", x, y, tile->cost, newCost, count); km += calculateH(previous); previous = start; //I am aware that the following code below could be refactored by 50% //since it's repeating itself with only a few changes double oldCost = tile->cost; double oldCostToTile, newCostToTile; double currentRHS, otherG; //Update CURRENT by finding its new minimum RHS-value from NEIGHBOURS std::vector<Tile*> neighbours(getNeighbours(tile)); for (int i = 0; i < neighbours.size(); i++){ tile->cost = oldCost; oldCostToTile = calculateC(tile, neighbours[i]); tile->cost = newCost; newCostToTile = calculateC(tile, neighbours[i]); currentRHS = tile->rhs; otherG = neighbours[i]->g; if (oldCostToTile > newCostToTile){ if (tile != goal){ tile->rhs = std::min(currentRHS, (newCostToTile + otherG)); } } else if (currentRHS == (oldCostToTile + otherG)){ if (tile != goal){ tile->rhs = getMinSuccessor(tile).second; } } } updateVertex(tile); //Update all NEIGHBOURING cells by finding their new min RHS-values from CURRENT for (int i = 0; i < neighbours.size(); i++){ tile->cost = oldCost; oldCostToTile = calculateC(tile, neighbours[i]); tile->cost = newCost; newCostToTile = calculateC(tile, neighbours[i]); currentRHS = neighbours[i]->rhs; otherG = tile->g; if (oldCostToTile > newCostToTile){ if (neighbours[i] != goal){ neighbours[i]->rhs = std::min(currentRHS, (newCostToTile + otherG)); } } else if (currentRHS == (oldCostToTile + otherG)){ if (neighbours[i] != goal){ neighbours[i]->rhs = getMinSuccessor(neighbours[i]).second; } } updateVertex(neighbours[i]); } computeShortestPath(); }