Beispiel #1
24
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();
}
Beispiel #2
0
/* 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;
}
Beispiel #4
0
/* 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;
}
Beispiel #5
0
/*
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();
}
Beispiel #6
0
/*
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();
}