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();
}