Esempio n. 1
0
int
plan_do_local(plan_t *plan, double lx, double ly, double plan_halfwidth)
{
  double gx, gy;
  int li, lj;
  int xmin,ymin,xmax,ymax;
  plan_cell_t* cell;
  double t0,t1;

  t0 = get_time();

  // Set bounds as directed
  xmin = PLAN_GXWX(plan, lx - plan_halfwidth);
  ymin = PLAN_GXWX(plan, ly - plan_halfwidth);
  xmax = PLAN_GXWX(plan, lx + plan_halfwidth);
  ymax = PLAN_GXWX(plan, ly + plan_halfwidth);
  plan_set_bounds(plan, xmin, ymin, xmax, ymax);

  // Reset plan costs (within the local patch)
  plan_reset(plan);

  // Find a local goal to pursue
  if(_plan_find_local_goal(plan, &gx, &gy, lx, ly) != 0)
  {
    puts("no local goal");
    return(-1);
  }

  //printf("local goal: %.3lf, %.3lf\n", gx, gy);

  plan->lpath_count = 0;
  if(_plan_update_plan(plan, lx, ly, gx, gy) != 0)
  {
    puts("local plan update failed");
    return(-1);
  }

  li = PLAN_GXWX(plan, lx);
  lj = PLAN_GYWY(plan, ly);

  // Cache the path
  for(cell = plan->cells + PLAN_INDEX(plan,li,lj);
      cell;
      cell = cell->plan_next)
  {
    if(plan->lpath_count >= plan->lpath_size)
    {
      plan->lpath_size *= 2;
      plan->lpath = (plan_cell_t**)realloc(plan->lpath,
                                           plan->lpath_size * sizeof(plan_cell_t*));
      assert(plan->lpath);
    }
    plan->lpath[plan->lpath_count++] = cell;
  }

  t1 = get_time();

  printf("computed local path: %.6lf\n", t1-t0);
  return(0);
}
Esempio n. 2
0
int
plan_do_global(plan_t *plan, double lx, double ly, double gx, double gy)
{
  plan_cell_t* cell;
  int li, lj;
  double t0,t1;

  t0 = get_time();

  // Set bounds to look over the entire grid
  plan_set_bounds(plan, 0, 0, plan->size_x - 1, plan->size_y - 1);

  // Reset plan costs
  plan_reset(plan);

  plan->path_count = 0;
  if(_plan_update_plan(plan, lx, ly, gx, gy) < 0)
  {
    // no path
    return(-1);
  }

  li = PLAN_GXWX(plan, lx);
  lj = PLAN_GYWY(plan, ly);

  // Cache the path
  for(cell = plan->cells + PLAN_INDEX(plan,li,lj);
      cell;
      cell = cell->plan_next)
  {
    if(plan->path_count >= plan->path_size)
    {
      plan->path_size *= 2;
      plan->path = (plan_cell_t**)realloc(plan->path,
                                          plan->path_size * sizeof(plan_cell_t*));
      assert(plan->path);
    }
    plan->path[plan->path_count++] = cell;
  }

  t1 = get_time();

  //printf("computed global path: %.6lf\n", t1-t0);

  return(0);
}
Esempio n. 3
0
int 
_plan_find_local_goal(plan_t *plan, double* gx, double* gy, 
                      double lx, double ly)
{
  int c;
  int c_min;
  double squared_d;
  double squared_d_min;
  int li,lj;
  plan_cell_t* cell;

  // Must already have computed a global goal
  if(plan->path_count == 0)
  {
    //puts("no global path");
    return(-1);
  }

  li = PLAN_GXWX(plan, lx);
  lj = PLAN_GYWY(plan, ly);

  assert(PLAN_VALID_BOUNDS(plan,li,lj));

  // Find the closest place to jump on the global path
  squared_d_min = DBL_MAX;
  c_min = -1;
  for(c=0;c<plan->path_count;c++)
  {
    cell = plan->path[c];
    squared_d = ((cell->ci - li) * (cell->ci - li) + 
                 (cell->cj - lj) * (cell->cj - lj));
    if(squared_d < squared_d_min)
    {
      squared_d_min = squared_d;
      c_min = c;
    }
  }
  assert(c_min > -1);

  // Follow the path to find the last cell that's inside the local planning
  // area
  for(c=c_min; c<plan->path_count; c++)
  {
    cell = plan->path[c];
    
    //printf("step %d: (%d,%d)\n", c, cell->ci, cell->cj);

    if((cell->ci < plan->min_x) || (cell->ci > plan->max_x) ||
       (cell->cj < plan->min_y) || (cell->cj > plan->max_y))
    {
      // Did we move at least one cell along the path?
      if(c == c_min)
      {
        // nope; the entire global path is outside the local region; can't
        // fix that here
        puts("global path not in local region");
        return(-1);
      }
      else
        break;
    }
  }

  assert(c > c_min);

  cell = plan->path[c-1];

  //printf("ci: %d cj: %d\n", cell->ci, cell->cj);
  *gx = PLAN_WXGX(plan, cell->ci);
  *gy = PLAN_WYGY(plan, cell->cj);
  
  return(0);
}
Esempio n. 4
0
// Generate the plan
int 
_plan_update_plan(plan_t *plan, double lx, double ly, double gx, double gy)
{
  int oi, oj, di, dj, ni, nj;
  int gi, gj, li,lj;
  float cost;
  plan_cell_t *cell, *ncell;
  char old_occ_state;
  float old_occ_dist;

  // Reset the queue
  heap_reset(plan->heap);

  // Initialize the goal cell
  gi = PLAN_GXWX(plan, gx);
  gj = PLAN_GYWY(plan, gy);

  // Initialize the start cell
  li = PLAN_GXWX(plan, lx);
  lj = PLAN_GYWY(plan, ly);

  //printf("planning from %d,%d to %d,%d\n", li,lj,gi,gj);

  if(!PLAN_VALID_BOUNDS(plan, gi, gj))
  {
    puts("goal out of bounds");
    return(-1);
  }
  
  if(!PLAN_VALID_BOUNDS(plan, li, lj))
  {
    puts("start out of bounds");
    return(-1);
  }

  // Latch and clear the obstacle state for the cell I'm in
  cell = plan->cells + PLAN_INDEX(plan, li, lj);
  old_occ_state = cell->occ_state_dyn;
  old_occ_dist = cell->occ_dist_dyn;
  cell->occ_state_dyn = -1;
  cell->occ_dist_dyn = (float) plan->max_radius;

  cell = plan->cells + PLAN_INDEX(plan, gi, gj);
  cell->plan_cost = 0;

  // Are we done?
  if((li == gi) && (lj == gj))
    return(0);
  
  plan_push(plan, cell);

  while (1)
  {
    float * p;
    cell = plan_pop(plan);
    if (cell == NULL)
      break;

    oi = cell->ci;
    oj = cell->cj;

    //printf("pop %d %d %f\n", cell->ci, cell->cj, cell->plan_cost);

    p = plan->dist_kernel_3x3;
    for (dj = -1; dj <= +1; dj++)
    {
      ncell = plan->cells + PLAN_INDEX(plan,oi-1,oj+dj);
      for (di = -1; di <= +1; di++, p++, ncell++)
      {
        if (!di && !dj)
          continue;
        //if (di && dj)
          //continue;
        
        ni = oi + di;
        nj = oj + dj;

        if (!PLAN_VALID_BOUNDS(plan, ni, nj))
          continue;

        if(ncell->mark)
          continue;

        if (ncell->occ_dist_dyn < plan->abs_min_radius)
          continue;

        cost = cell->plan_cost;
        if(ncell->lpathmark)
          cost += (float) ((*p) * plan->hysteresis_factor);
        else
          cost += *p;

        if(ncell->occ_dist_dyn < plan->max_radius)
          cost += (float) (plan->dist_penalty * (plan->max_radius - ncell->occ_dist_dyn));

        if(cost < ncell->plan_cost)
        {
          ncell->plan_cost = cost;
          ncell->plan_next = cell;

          plan_push(plan, ncell);
        }
      }
    }
  }

  // Restore the obstacle state for the cell I'm in
  cell = plan->cells + PLAN_INDEX(plan, li, lj);
  cell->occ_state_dyn = old_occ_state;
  cell->occ_dist_dyn = old_occ_dist;

  if(!cell->plan_next)
  {
    //puts("never found start");
    return(-1);
  }
  else
    return(0);
}
Esempio n. 5
0
// Generate the plan
int 
_plan_update_plan(plan_t *plan, double lx, double ly, double gx, double gy)
{
  int oi, oj, di, dj, ni, nj;
  int gi, gj, li,lj;
  float cost;
  plan_cell_t *cell, *ncell;

  // Reset the queue
  heap_reset(plan->heap);

  // Initialize the goal cell
  gi = PLAN_GXWX(plan, gx);
  gj = PLAN_GYWY(plan, gy);

  // Initialize the start cell
  li = PLAN_GXWX(plan, lx);
  lj = PLAN_GYWY(plan, ly);

  //printf("planning from %d,%d to %d,%d\n", li,lj,gi,gj);

  if(!PLAN_VALID_BOUNDS(plan, gi, gj))
  {
    puts("goal out of bounds");
    return(-1);
  }
  
  if(!PLAN_VALID_BOUNDS(plan, li, lj))
  {
    puts("start out of bounds");
    return(-1);
  }

  cell = plan->cells + PLAN_INDEX(plan, gi, gj);
  cell->plan_cost = 0;
  plan_push(plan, cell);

  while (1)
  {
    float * p;
    cell = plan_pop(plan);
    if (cell == NULL)
      break;

    oi = cell->ci;
    oj = cell->cj;

    //printf("pop %d %d %f\n", cell->ci, cell->cj, cell->plan_cost);

    p = plan->dist_kernel_3x3;
    for (dj = -1; dj <= +1; dj++)
    {
      ncell = plan->cells + PLAN_INDEX(plan,oi-1,oj+dj);
      for (di = -1; di <= +1; di++, p++, ncell++)
      {
        if (!di && !dj)
          continue;
        //if (di && dj)
          //continue;
        
        ni = oi + di;
        nj = oj + dj;

        if (!PLAN_VALID_BOUNDS(plan, ni, nj))
          continue;

        if(ncell->mark)
          continue;

        if (ncell->occ_dist_dyn < plan->abs_min_radius)
          continue;

        cost = cell->plan_cost + *p;

        if(ncell->occ_dist_dyn < plan->max_radius)
          cost += plan->dist_penalty * (plan->max_radius - ncell->occ_dist_dyn);

        if(cost < ncell->plan_cost)
        {
          ncell->plan_cost = cost;
          ncell->plan_next = cell;

          plan_push(plan, ncell);
        }
      }
    }
  }

  cell = plan->cells + PLAN_INDEX(plan, li, lj);
  if(!cell->plan_next)
  {
    puts("never found start");
    return(-1);
  }
  else
    return(0);
}
Esempio n. 6
0
// Generate a path to the goal
void plan_update_waypoints(plan_t *plan, double px, double py)
{
  double dist;
  int ni, nj;
  plan_cell_t *cell, *ncell;

  plan->waypoint_count = 0;

  ni = PLAN_GXWX(plan, px);
  nj = PLAN_GYWY(plan, py);

  // Can't plan a path if we're off the map
  if(!PLAN_VALID(plan,ni,nj))
    return;

  cell = plan->cells + PLAN_INDEX(plan, ni, nj);

  while (cell != NULL)
  {
    if (plan->waypoint_count >= plan->waypoint_size)
    {
      plan->waypoint_size *= 2;
      plan->waypoints = realloc(plan->waypoints,
                                plan->waypoint_size * sizeof(plan->waypoints[0]));
    }
    
    plan->waypoints[plan->waypoint_count++] = cell;

    if (cell->plan_next == NULL)
    {
      // done
      break;
    }

    // Find the farthest cell in the path that is reachable from the
    // currrent cell.
    dist = 0;
    for(ncell = cell; ncell->plan_next != NULL; ncell = ncell->plan_next)
    {
      if(dist > 0.50)
      {
        if(!plan_test_reachable(plan, cell, ncell->plan_next))
          break;
      }
      dist += plan->scale;
    }
    if(ncell == cell)
    {
      break;
    }
    
    cell = ncell;
  }

  if(cell && (cell->plan_cost > 0))
  {
    // no path
    plan->waypoint_count = 0;
  }
  
  return;
}
Esempio n. 7
0
// Test to see if once cell is reachable from another.
// This could be improved.
int plan_test_reachable(plan_t *plan, plan_cell_t *cell_a, plan_cell_t *cell_b)
{
  int i, j;
  int ai, aj, bi, bj;
  double ox, oy, oa;
  double dx, dy;
  plan_cell_t *cell;

  ai = cell_a->ci;
  aj = cell_a->cj;
  bi = cell_b->ci;
  bj = cell_b->cj;

  ox = PLAN_WXGX(plan, ai);
  oy = PLAN_WYGY(plan, aj);
  oa = atan2(bj - aj, bi - ai);
  
  if (fabs(cos(oa)) > fabs(sin(oa)))
  {
    dy = tan(oa) * plan->scale;

    if (ai < bi)
    {
      for (i = ai; i < bi; i++)
      {
        j = PLAN_GYWY(plan, oy + (i - ai) * dy);
        if (PLAN_VALID(plan, i, j))
        {
          cell = plan->cells + PLAN_INDEX(plan, i, j);
          if (cell->occ_dist < plan->abs_min_radius)
            return 0;
        }
      }
    }
    else
    {
      for (i = ai; i > bi; i--)
      {
        j = PLAN_GYWY(plan, oy + (i - ai) * dy);
        if (PLAN_VALID(plan, i, j))
        {
          cell = plan->cells + PLAN_INDEX(plan, i, j);
          if (cell->occ_dist < plan->abs_min_radius)
            return 0;
        }
      }
    }
  }
  else
  {
    dx = tan(M_PI/2 - oa) * plan->scale;

    if (aj < bj)
    {
      for (j = aj; j < bj; j++)
      {
        i = PLAN_GXWX(plan, ox + (j - aj) * dx);
        if (PLAN_VALID(plan, i, j))
        {
          cell = plan->cells + PLAN_INDEX(plan, i, j);
          if (cell->occ_dist < plan->abs_min_radius)
            return 0;
        }
      }
    }
    else
    {
      for (j = aj; j > bj; j--)
      {
        i = PLAN_GXWX(plan, ox + (j - aj) * dx);
        if (PLAN_VALID(plan, i, j))
        {
          cell = plan->cells + PLAN_INDEX(plan, i, j);
          if (cell->occ_dist < plan->abs_min_radius)
            return 0;
        }
      }
    }
  }
  return 1;
}