Ejemplos de plan_pop en C++ (Cpp)

Ejemplo n.º 1

Archivo: plan_plan.c Proyecto: Arkapravo/Player-3.0.2

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

Ejemplo n.º 2

Archivo: plan_plan.c Proyecto: uml-robotics/player-2.1.3

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