Revision: 4480
http://playerstage.svn.sourceforge.net/playerstage/?rev=4480&view=rev
Author: gerkey
Date: 2008-04-04 18:55:18 -0700 (Fri, 04 Apr 2008)
Log Message:
-----------
more speedups
Modified Paths:
--------------
code/player/trunk/server/drivers/planner/wavefront/plan.c
code/player/trunk/server/drivers/planner/wavefront/plan.h
code/player/trunk/server/drivers/planner/wavefront/plan_plan.c
code/player/trunk/server/drivers/planner/wavefront/test.c
code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-05
00:31:33 UTC (rev 4479)
+++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-05
01:55:18 UTC (rev 4480)
@@ -61,6 +61,8 @@
plan->des_min_radius = des_min_radius;
plan->max_radius = max_radius;
+ plan->dist_kernel_width = 0;
+ plan->dist_kernel = NULL;
plan->dist_penalty = dist_penalty;
//plan->queue_start = 0;
@@ -77,7 +79,43 @@
return plan;
}
+void
+plan_compute_dist_kernel(plan_t* plan)
+{
+ int i,j;
+ float* p;
+ // Compute variable sized kernel, for use in propagating distance from
+ // obstacles
+ plan->dist_kernel_width = 1 + 2 * (int)ceil(plan->max_radius / plan->scale);
+ plan->dist_kernel = (float*)realloc(plan->dist_kernel,
+ sizeof(float) *
+ plan->dist_kernel_width *
+ plan->dist_kernel_width);
+ assert(plan->dist_kernel);
+
+ p = plan->dist_kernel;
+ for(j=-plan->dist_kernel_width/2;j<plan->dist_kernel_width/2;j++)
+ {
+ for(i=-plan->dist_kernel_width/2;i<plan->dist_kernel_width/2;i++,p++)
+ {
+ *p = sqrt(i*i+j*j) * plan->scale;
+ // also compute a 3x3 kernel, used when propagating distance from goal
+ if((abs(i) <= 1) && (abs(j) <= 1))
+ plan->dist_kernel_3x3[i+1][j+1] = *p;
+ }
+ }
+ for(j=0;j<3;j++)
+ {
+ for(i=0;i<3;i++)
+ {
+ printf("%.3f ", plan->dist_kernel_3x3[i][j]);
+ }
+ puts("");
+ }
+}
+
+
// Destroy a planner
void plan_free(plan_t *plan)
{
@@ -85,6 +123,8 @@
free(plan->cells);
heap_free(plan->heap);
free(plan->waypoints);
+ if(plan->dist_kernel)
+ free(plan->dist_kernel);
free(plan);
return;
@@ -97,16 +137,16 @@
int i, j;
plan_cell_t *cell;
+ cell = plan->cells;
for (j = 0; j < plan->size_y; j++)
{
- for (i = 0; i < plan->size_x; i++)
+ for (i = 0; i < plan->size_x; i++, cell++)
{
- cell = plan->cells + PLAN_INDEX(plan, i, j);
cell->ci = i;
cell->cj = j;
cell->occ_state = 0;
cell->occ_dist = plan->max_radius;
- cell->plan_cost = 1e12;
+ cell->plan_cost = PLAN_MAX_COST;
cell->plan_next = NULL;
}
}
@@ -215,35 +255,40 @@
plan_update_cspace_naive(plan_t* plan)
{
int i, j;
- int di, dj, dn;
- double r;
+ int di, dj;
+ float* p;
plan_cell_t *cell, *ncell;
PLAYER_MSG0(2,"Generating C-space....");
- dn = (int) ceil(plan->max_radius / plan->scale);
for (j = plan->min_y; j <= plan->max_y; j++)
{
- for (i = plan->min_x; i <= plan->max_x; i++)
+ cell = plan->cells + PLAN_INDEX(plan, 0, j);
+ for (i = plan->min_x; i <= plan->max_x; i++, cell++)
{
- cell = plan->cells + PLAN_INDEX(plan, i, j);
if (cell->occ_state < 0)
continue;
cell->occ_dist = FLT_MAX;
- for (dj = -dn; dj <= +dn; dj++)
+ p = plan->dist_kernel;
+ for (dj = -plan->dist_kernel_width/2;
+ dj <= plan->dist_kernel_width/2;
+ dj++)
{
- for (di = -dn; di <= +dn; di++)
+ di = -plan->dist_kernel_width/2;
+ //ncell = plan->cells + PLAN_INDEX(plan, i + di, j + dj);
+ ncell = cell + di + dj*plan->size_x;
+ for (; di <= plan->dist_kernel_width/2;
+ di++, ncell++, p++)
{
if (!PLAN_VALID_BOUNDS(plan, i + di, j + dj))
continue;
- ncell = plan->cells + PLAN_INDEX(plan, i + di, j + dj);
- r = plan->scale * sqrt(di * di + dj * dj);
- if (r < ncell->occ_dist)
- ncell->occ_dist = r;
+ //if(*p < ncell->occ_dist)
+ if(0.1 < ncell->occ_dist)
+ ncell->occ_dist = *p;
}
}
}
@@ -317,9 +362,6 @@
int ci, cj;
plan_cell_t* cell;
- ci = PLAN_GXWX(plan, lx);
- cj = PLAN_GYWY(plan, ly);
-
pixels = (guchar*)malloc(sizeof(guchar)*plan->size_x*plan->size_y*3);
p=0;
@@ -363,30 +405,36 @@
}
}
- cell = plan->cells + PLAN_INDEX(plan, ci, cj);
- while (cell != NULL)
+ ci = PLAN_GXWX(plan, lx);
+ cj = PLAN_GYWY(plan, ly);
+
+ if(PLAN_VALID_BOUNDS(plan,ci,cj))
{
- paddr = 3*PLAN_INDEX(plan,cell->ci,plan->size_y - cell->cj - 1);
- pixels[paddr] = 0;
- pixels[paddr+1] = 255;
- pixels[paddr+2] = 0;
+ cell = plan->cells + PLAN_INDEX(plan, ci, cj);
+ while (cell != NULL)
+ {
+ paddr = 3*PLAN_INDEX(plan,cell->ci,plan->size_y - cell->cj - 1);
+ pixels[paddr] = 0;
+ pixels[paddr+1] = 255;
+ pixels[paddr+2] = 0;
- cell = cell->plan_next;
- }
+ cell = cell->plan_next;
+ }
- for(p=0;p<plan->waypoint_count;p++)
- {
- cell = plan->waypoints[p];
- for(j=-3;j<=3;j++)
+ for(p=0;p<plan->waypoint_count;p++)
{
- cj = cell->cj + j;
- for(i=-3;i<=3;i++)
+ cell = plan->waypoints[p];
+ for(j=-3;j<=3;j++)
{
- ci = cell->ci + i;
- paddr = 3*PLAN_INDEX(plan,ci,plan->size_y - cj - 1);
- pixels[paddr] = 255;
- pixels[paddr+1] = 0;
- pixels[paddr+2] = 255;
+ cj = cell->cj + j;
+ for(i=-3;i<=3;i++)
+ {
+ ci = cell->ci + i;
+ paddr = 3*PLAN_INDEX(plan,ci,plan->size_y - cj - 1);
+ pixels[paddr] = 255;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 255;
+ }
}
}
}
@@ -405,119 +453,6 @@
}
#endif
-#if 0
-void
-plan_update_cspace_dp(plan_t* plan)
-{
- int i, j;
- int di, dj, dn;
- double r;
- plan_cell_t *cell, *ncell;
- //heap_t* Q;
- plan_cell_t** Q;
- int qhead, qtail;
-
- PLAYER_MSG0(2,"Generating C-space....");
-
- // We'll look this far away from obstacles when updating cell costs
- dn = (int) ceil(plan->max_radius / plan->scale);
-
- // We'll need space for, at most, dn^2 cells in our queue (which is a
- // binary heap).
- //Q = heap_alloc(dn*dn, NULL);
- Q = (plan_cell_t**)malloc(sizeof(plan_cell_t*)*dn*dn);
- assert(Q);
-
- // Make space for the marks that we'll use.
- if(plan->marks_size != plan->size_x*plan->size_y)
- {
- plan->marks_size = plan->size_x*plan->size_y;
- plan->marks = (unsigned char*)realloc(plan->marks,
- sizeof(unsigned char*) *
- plan->marks_size);
- }
- assert(plan->marks);
-
- // For each obstacle (or unknown cell), spread a wave out in all
- // directions, updating occupancy distances (inverse costs) on the way.
- // Don't ever raise the occupancy distance of a cell.
- for (j = 0; j < plan->size_y; j++)
- {
- for (i = 0; i < plan->size_x; i++)
- {
- cell = plan->cells + PLAN_INDEX(plan, i, j);
- if (cell->occ_state < 0)
- continue;
-
- //cell->occ_dist = plan->max_radius;
- cell->occ_dist = 0.0;
-
- // clear the marks
- memset(plan->marks,0,sizeof(unsigned char)*plan->size_x*plan->size_y);
-
- // Start with the current cell
- //heap_reset(Q);
- //heap_insert(Q, cell->occ_dist, cell);
- qhead = 0;
- Q[qhead] = cell;
- qtail = 1;
-
- //while(!heap_empty(Q))
- while(qtail != qhead)
- {
- //ncell = heap_extract_max(Q);
- ncell = Q[qhead++];
-
- // Mark it, so we don't come back
- plan->marks[PLAN_INDEX(plan, ncell->ci, ncell->cj)] = 1;
-
- // Is this cell an obstacle or unknown cell (and not the initial
- // cell? If so, don't bother updating its cost here.
- if((ncell != cell) && (ncell->occ_state >= 0))
- continue;
-
- // How far are we from the obstacle cell we started with?
- r = plan->scale * hypot(ncell->ci - cell->ci,
- ncell->cj - cell->cj);
-
- // Are we past the distance at which we care?
- if(r > plan->max_radius)
- continue;
-
- // Update the occupancy distance if we're closer
- if(r < ncell->occ_dist)
- {
- ncell->occ_dist = r;
- // Also put its neighbors on the queue for processing
- for(dj = -1; dj <= 1; dj+= 2)
- {
- for(di = -1; di <= 1; di+= 2)
- {
- if (!PLAN_VALID(plan, ncell->ci + di, ncell->cj + dj))
- continue;
- // Have we already seen this cell?
- if(plan->marks[PLAN_INDEX(plan, ncell->ci + di, ncell->cj + dj)])
- continue;
- // Add it to the queue
- /*
- heap_insert(Q, ncell->occ_dist,
- plan->cells + PLAN_INDEX(plan,
- ncell->ci+di,
- ncell->cj+dj));
- */
- Q[qtail++] = plan->cells + PLAN_INDEX(plan,
- ncell->ci+di,
- ncell->cj+dj);
- }
- }
- }
- }
- }
- }
- //heap_free(Q);
-}
-#endif
-
// Construct the configuration space from the occupancy grid.
// This treats both occupied and unknown cells as bad.
//
Modified: code/player/trunk/server/drivers/planner/wavefront/plan.h
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-05
00:31:33 UTC (rev 4479)
+++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-05
01:55:18 UTC (rev 4480)
@@ -66,8 +66,11 @@
// The grid data
plan_cell_t *cells;
- unsigned char* marks;
- size_t marks_size;
+
+ // Distance penalty kernel, pre-computed in plan_compute_dist_kernel();
+ float* dist_kernel;
+ int dist_kernel_width;
+ float dist_kernel_3x3[3][3];
// Queue of cells to update
//int queue_start, queue_len, queue_size;
@@ -84,6 +87,8 @@
plan_t *plan_alloc(double abs_min_radius, double des_min_radius,
double max_radius, double dist_penalty);
+void plan_compute_dist_kernel(plan_t* plan);
+
// Destroy a planner
void plan_free(plan_t *plan);
@@ -106,7 +111,7 @@
void plan_update_cspace(plan_t *plan, const char* cachefile);
// Generate the plan
-void plan_update_plan(plan_t *plan, double gx, double gy);
+void plan_update_plan(plan_t *plan, double lx, double ly, double gx, double
gy);
// Generate a path to the goal
void plan_update_waypoints(plan_t *plan, double px, double py);
Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c
2008-04-05 00:31:33 UTC (rev 4479)
+++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c
2008-04-05 01:55:18 UTC (rev 4480)
@@ -20,18 +20,20 @@
// Generate the plan
-void plan_update_plan(plan_t *plan, double gx, double gy)
+void
+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 grid
+ cell = plan->cells;
for (nj = 0; nj < plan->size_y; nj++)
{
- for (ni = 0; ni < plan->size_x; ni++)
+ for (ni = 0; ni < plan->size_x; ni++,cell++)
{
- cell = plan->cells + PLAN_INDEX(plan, ni, nj);
cell->plan_cost = PLAN_MAX_COST;
cell->plan_next = NULL;
}
@@ -43,13 +45,20 @@
heap_reset(plan->heap);
// Initialize the goal cell
- ni = PLAN_GXWX(plan, gx);
- nj = PLAN_GYWY(plan, gy);
+ gi = PLAN_GXWX(plan, gx);
+ gj = PLAN_GYWY(plan, gy);
- if (!PLAN_VALID_BOUNDS(plan, ni, nj))
+ if (!PLAN_VALID_BOUNDS(plan, gi, gj))
return;
+
+ // Initialize the start cell
+ li = PLAN_GXWX(plan, lx);
+ lj = PLAN_GYWY(plan, ly);
+
+ if (!PLAN_VALID_BOUNDS(plan, li, lj))
+ return;
- cell = plan->cells + PLAN_INDEX(plan, ni, nj);
+ cell = plan->cells + PLAN_INDEX(plan, gi, gj);
cell->plan_cost = 0;
plan_push(plan, cell);
@@ -83,11 +92,15 @@
if (ncell->occ_dist < plan->abs_min_radius)
continue;
+ /*
if(di && dj)
cost = cell->plan_cost + plan->scale * M_SQRT2;
else
cost = cell->plan_cost + plan->scale;
+ */
+ cost = cell->plan_cost + plan->dist_kernel_3x3[di+1][dj+1];
+
if (ncell->occ_dist < plan->max_radius)
cost += plan->dist_penalty * (plan->max_radius - ncell->occ_dist);
@@ -95,6 +108,10 @@
{
ncell->plan_cost = cost;
ncell->plan_next = cell;
+
+ // Are we done?
+ if((ncell->ci == li) && (ncell->cj == lj))
+ return;
plan_push(plan, ncell);
}
}
Modified: code/player/trunk/server/drivers/planner/wavefront/test.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-05
00:31:33 UTC (rev 4479)
+++ code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-05
01:55:18 UTC (rev 4480)
@@ -61,6 +61,7 @@
dist_penalty)));
plan->scale = res;
+ plan_compute_dist_kernel(plan);
plan->size_x = sx;
plan->size_y = sy;
plan->origin_x = 0.0;
@@ -97,7 +98,7 @@
// compute costs to the new goal
t_p0 = get_time();
- plan_update_plan(plan, gx, gy);
+ plan_update_plan(plan, lx, ly, gx, gy);
t_p1 = get_time();
// compute a path to the goal from the current position
Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
2008-04-05 00:31:33 UTC (rev 4479)
+++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
2008-04-05 01:55:18 UTC (rev 4480)
@@ -507,6 +507,7 @@
// Got new map info pushed to us. We'll save this info and get the new
// map.
this->plan->scale = info->scale;
+ plan_compute_dist_kernel(this->plan);
this->plan->size_x = info->width;
this->plan->size_y = info->height;
this->plan->origin_x = info->origin.px;
@@ -767,7 +768,8 @@
#endif
// compute costs to the new goal
- plan_update_plan(this->plan, this->target_x, this->target_y);
+ plan_update_plan(this->plan, this->localize_x, this->localize_x,
+ this->target_x, this->target_y);
// compute a path to the goal from the current position
plan_update_waypoints(this->plan, this->localize_x, this->localize_y);
@@ -1124,6 +1126,7 @@
{
PLAYER_WARN("failed to get map info");
this->plan->scale = 0.1;
+ plan_compute_dist_kernel(this->plan);
this->plan->size_x = 0;
this->plan->size_y = 0;
this->plan->origin_x = 0.0;
@@ -1135,6 +1138,7 @@
// copy in the map info
this->plan->scale = info->scale;
+ plan_compute_dist_kernel(this->plan);
this->plan->size_x = info->width;
this->plan->size_y = info->height;
this->plan->origin_x = info->origin.px;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.net email is sponsored by the 2008 JavaOne(SM) Conference
Register now and save $200. Hurry, offer ends at 11:59 p.m.,
Monday, April 7! Use priority code J8TLD2.
http://ad.doubleclick.net/clk;198757673;13503038;p?http://java.sun.com/javaone
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
