Revision: 6360
http://playerstage.svn.sourceforge.net/playerstage/?rev=6360&view=rev
Author: gbiggs
Date: 2008-04-18 01:07:57 -0700 (Fri, 18 Apr 2008)
Log Message:
-----------
Merging changes 4470:6287 from trunk
Modified Paths:
--------------
code/player/branches/cmake/CMake_Todo.txt
code/player/branches/cmake/server/drivers/planner/wavefront/CMakeLists.txt
code/player/branches/cmake/server/drivers/planner/wavefront/plan.c
code/player/branches/cmake/server/drivers/planner/wavefront/plan.h
code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
Added Paths:
-----------
code/player/branches/cmake/server/drivers/planner/wavefront/Makefile.test
code/player/branches/cmake/server/drivers/planner/wavefront/heap.c
code/player/branches/cmake/server/drivers/planner/wavefront/heap.h
code/player/branches/cmake/server/drivers/planner/wavefront/test.c
Modified: code/player/branches/cmake/CMake_Todo.txt
===================================================================
--- code/player/branches/cmake/CMake_Todo.txt 2008-04-18 07:54:24 UTC (rev
6359)
+++ code/player/branches/cmake/CMake_Todo.txt 2008-04-18 08:07:57 UTC (rev
6360)
@@ -10,4 +10,5 @@
- Some installable cmake modules with macros for making plugin drivers, etc
- Clean out/replace any stragglers from autotools (including README and co)
- Drivers being disabled by a check used to correctly force their option to
off. Figure out why this broke. Fix it.
-- Figure out why C++ tests and C++ examples don't link properly when building
as static libs, even though the server and utils do.
\ No newline at end of file
+- Figure out why C++ tests and C++ examples don't link properly when building
as static libs, even though the server and utils do.
+- Add CMakeLists equivalent of the wavefront driver's Makefile.test (and get
rid of Makefile.test)
\ No newline at end of file
Modified:
code/player/branches/cmake/server/drivers/planner/wavefront/CMakeLists.txt
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/CMakeLists.txt
2008-04-18 07:54:24 UTC (rev 6359)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/CMakeLists.txt
2008-04-18 08:07:57 UTC (rev 6360)
@@ -3,4 +3,4 @@
wavefront_includeDir wavefront_libDir wavefront_linkFlags wavefront_cFlags)
PLAYERDRIVER_ADD_DRIVER (wavefront build_wavefront
"${wavefront_includeDir}" "${wavefront_libDir}" "${wavefront_linkFlags}"
"${wavefront_cFlags}"
- plan.c plan_plan.c plan_waypoint.c wavefront.cc)
\ No newline at end of file
+ plan.c plan_plan.c plan_waypoint.c wavefront.cc heap.c)
\ No newline at end of file
Copied:
code/player/branches/cmake/server/drivers/planner/wavefront/Makefile.test (from
rev 6287, code/player/trunk/server/drivers/planner/wavefront/Makefile.test)
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/Makefile.test
(rev 0)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/Makefile.test
2008-04-18 08:07:57 UTC (rev 6360)
@@ -0,0 +1,11 @@
+#CFLAGS = -I. -I../../../.. -Wall -Werror -O3 `pkg-config --cflags
gdk-pixbuf-2.0`
+CFLAGS = -I. -I../../../.. -Wall -Werror -g `pkg-config --cflags
gdk-pixbuf-2.0`
+LIBS = -L../../../../libplayercore/.libs -lplayererror `pkg-config --libs
gdk-pixbuf-2.0`
+
+SRCS = test.c plan.c plan_plan.c plan_waypoint.c heap.c
+
+test: $(SRCS) plan.h heap.h
+ gcc $(CFLAGS) -o $@ $(SRCS) $(LIBS)
+
+clean:
+ rm -f test
Copied: code/player/branches/cmake/server/drivers/planner/wavefront/heap.c
(from rev 6287, code/player/trunk/server/drivers/planner/wavefront/heap.c)
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/heap.c
(rev 0)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/heap.c
2008-04-18 08:07:57 UTC (rev 6360)
@@ -0,0 +1,166 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2008-
+ * Brian Gerkey [EMAIL PROTECTED]
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/*
+ * An implementation of a heap, as seen in "Introduction to Algorithms," by
+ * Cormen, Leiserson, and Rivest, pages 140-152.
+ */
+#include <stdlib.h>
+#include <assert.h>
+#include <stdio.h>
+
+#include "heap.h"
+
+heap_t*
+heap_alloc(int size, heap_free_elt_fn_t free_fn)
+{
+ heap_t* h;
+
+ h = calloc(1,sizeof(heap_t));
+ assert(h);
+ h->size = size;
+ h->free_fn = free_fn;
+ h->A = calloc(h->size,sizeof(double));
+ assert(h->A);
+ h->data = calloc(h->size,sizeof(void*));
+ assert(h->data);
+ h->len = 0;
+
+ return(h);
+}
+
+void
+heap_free(heap_t* h)
+{
+ if(h->free_fn)
+ {
+ while(!heap_empty(h))
+ (*h->free_fn)(heap_extract_max(h));
+ }
+ free(h->data);
+ free(h->A);
+ free(h);
+}
+
+void
+heap_heapify(heap_t* h, int i)
+{
+ int l, r;
+ int largest;
+ double tmp;
+ void* tmp_data;
+
+ l = HEAP_LEFT(i);
+ r = HEAP_RIGHT(i);
+
+ if((l < h->len) && (h->A[l] > h->A[i]))
+ largest = l;
+ else
+ largest = i;
+
+ if((r < h->len) && (h->A[r] > h->A[largest]))
+ largest = r;
+
+ if(largest != i)
+ {
+ tmp = h->A[i];
+ tmp_data = h->data[i];
+ h->A[i] = h->A[largest];
+ h->data[i] = h->data[largest];
+ h->A[largest] = tmp;
+ h->data[largest] = tmp_data;
+ heap_heapify(h,largest);
+ }
+}
+
+int
+heap_empty(heap_t* h)
+{
+ return(h->len == 0);
+}
+
+void*
+heap_extract_max(heap_t* h)
+{
+ void* max;
+
+ assert(h->len > 0);
+
+ max = h->data[0];
+ h->A[0] = h->A[h->len - 1];
+ h->data[0] = h->data[h->len - 1];
+ h->len--;
+ heap_heapify(h,0);
+ return(max);
+}
+
+void
+heap_insert(heap_t* h, double key, void* data)
+{
+ int i;
+
+ if(h->len == h->size)
+ {
+ h->size *= 2;
+ h->A = realloc(h->A, h->size * sizeof(double));
+ assert(h->A);
+ h->data = realloc(h->data, h->size * sizeof(void*));
+ assert(h->data);
+ }
+
+ h->len++;
+ i = h->len - 1;
+
+ while((i > 0) && (h->A[HEAP_PARENT(i)] < key))
+ {
+ h->A[i] = h->A[HEAP_PARENT(i)];
+ h->data[i] = h->data[HEAP_PARENT(i)];
+ i = HEAP_PARENT(i);
+ }
+ h->A[i] = key;
+ h->data[i] = data;
+}
+
+int
+heap_valid(heap_t* h)
+{
+ int i;
+ for(i=1;i<h->len;i++)
+ {
+ if(h->A[HEAP_PARENT(i)] < h->A[i])
+ return(0);
+ }
+ return(1);
+}
+
+void
+heap_reset(heap_t* h)
+{
+ h->len = 0;
+}
+
+void
+heap_dump(heap_t* h)
+{
+ int i;
+ for(i=0;i<h->len;i++)
+ printf("%d: %f\n", i, h->A[i]);
+}
Copied: code/player/branches/cmake/server/drivers/planner/wavefront/heap.h
(from rev 6287, code/player/trunk/server/drivers/planner/wavefront/heap.h)
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/heap.h
(rev 0)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/heap.h
2008-04-18 08:07:57 UTC (rev 6360)
@@ -0,0 +1,65 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2008-
+ * Brian Gerkey [EMAIL PROTECTED]
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/*
+ * An implementation of a heap, as seen in "Introduction to Algorithms," by
+ * Cormen, Leiserson, and Rivest, pages 140-152.
+ */
+
+#ifndef _HEAP_H_
+#define _HEAP_H_
+
+#define HEAP_PARENT(i) ((i)/2)
+#define HEAP_LEFT(i) (2*(i))
+#define HEAP_RIGHT(i) (2*(i)+1)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct heap;
+
+typedef void (*heap_free_elt_fn_t) (void* elt);
+
+typedef struct heap
+{
+ int len;
+ int size;
+ heap_free_elt_fn_t free_fn;
+ double* A;
+ void** data;
+} heap_t;
+
+heap_t* heap_alloc(int size, heap_free_elt_fn_t free_fn);
+void heap_free(heap_t* h);
+void heap_heapify(heap_t* h, int i);
+void* heap_extract_max(heap_t* h);
+void heap_insert(heap_t* h, double key, void* data);
+void heap_dump(heap_t* h);
+int heap_valid(heap_t* h);
+int heap_empty(heap_t* h);
+void heap_reset(heap_t* h);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
Modified: code/player/branches/cmake/server/drivers/planner/wavefront/plan.c
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/plan.c
2008-04-18 07:54:24 UTC (rev 6359)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan.c
2008-04-18 08:07:57 UTC (rev 6360)
@@ -46,77 +46,207 @@
plan = calloc(1, sizeof(plan_t));
- plan->size_x = 0;
- plan->size_y = 0;
- plan->scale = 0.0;
-
- plan->min_x = 0;
- plan->min_y = 0;
- plan->max_x = 0;
- plan->max_y = 0;
-
plan->abs_min_radius = abs_min_radius;
plan->des_min_radius = des_min_radius;
plan->max_radius = max_radius;
plan->dist_penalty = dist_penalty;
- plan->queue_start = 0;
- plan->queue_len = 0;
- plan->queue_size = 400000; // HACK: FIX
- plan->queue = calloc(plan->queue_size, sizeof(plan->queue[0]));
+ plan->heap = heap_alloc(PLAN_DEFAULT_HEAP_SIZE, (heap_free_elt_fn_t)NULL);
+ assert(plan->heap);
- plan->waypoint_count = 0;
+ plan->path_size = 1000;
+ plan->path = calloc(plan->path_size, sizeof(plan->path[0]));
+
+ plan->lpath_size = 100;
+ plan->lpath = calloc(plan->lpath_size, sizeof(plan->lpath[0]));
+
plan->waypoint_size = 100;
plan->waypoints = calloc(plan->waypoint_size, sizeof(plan->waypoints[0]));
return plan;
}
+void
+plan_set_obstacles(plan_t* plan, double* obs, size_t num)
+{
+ size_t i;
+ int di,dj;
+ float* p;
+ plan_cell_t* cell, *ncell;
+ // Remove any previous obstacles
+ for(i=0;i<plan->obs_pts_num;i++)
+ {
+ cell = plan->cells +
+ PLAN_INDEX(plan,plan->obs_pts[2*i],plan->obs_pts[2*i+1]);
+ cell->occ_state_dyn = cell->occ_state;
+ cell->occ_dist_dyn = cell->occ_dist;
+ }
+
+ // Do we need more room?
+ if(num > plan->obs_pts_size)
+ {
+ plan->obs_pts_size = num;
+ plan->obs_pts = (unsigned short*)realloc(plan->obs_pts,
+ sizeof(unsigned short) * 2 *
+ plan->obs_pts_size);
+ assert(plan->obs_pts);
+ }
+
+ // Copy and expand costs around them
+ plan->obs_pts_num = num;
+ for(i=0;i<plan->obs_pts_num;i++)
+ {
+ // Convert to grid coords
+ int gx,gy;
+ gx = PLAN_GXWX(plan, obs[2*i]);
+ gy = PLAN_GYWY(plan, obs[2*i+1]);
+ plan->obs_pts[2*i] = gx;
+ plan->obs_pts[2*i+1] = gy;
+
+ cell = plan->cells + PLAN_INDEX(plan,gx,gy);
+
+ cell->occ_state_dyn = 1;
+ cell->occ_dist_dyn = 0.0;
+
+ p = plan->dist_kernel;
+ for (dj = -plan->dist_kernel_width/2;
+ dj <= plan->dist_kernel_width/2;
+ dj++)
+ {
+ ncell = cell + -plan->dist_kernel_width/2 + dj*plan->size_x;
+ for (di = -plan->dist_kernel_width/2;
+ di <= plan->dist_kernel_width/2;
+ di++, p++, ncell++)
+ {
+ if(!PLAN_VALID_BOUNDS(plan,cell->ci+di,cell->cj+dj))
+ continue;
+
+ if(*p < ncell->occ_dist_dyn)
+ ncell->occ_dist_dyn = *p;
+ }
+ }
+ }
+}
+
+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
+ p = plan->dist_kernel_3x3;
+ for(j=-1;j<=1;j++)
+ {
+ for(i=-1;i<=1;i++,p++)
+ {
+ *p = sqrt(i*i+j*j) * plan->scale;
+ }
+ }
+}
+
+
// Destroy a planner
void plan_free(plan_t *plan)
{
if (plan->cells)
free(plan->cells);
- free(plan->queue);
+ heap_free(plan->heap);
free(plan->waypoints);
+ if(plan->dist_kernel)
+ free(plan->dist_kernel);
free(plan);
return;
}
-
-// Reset the plan
-void plan_reset(plan_t *plan)
+// Initialize the plan
+void plan_init(plan_t *plan,
+ double res, double sx, double sy, double ox, double oy)
{
int i, j;
plan_cell_t *cell;
+ plan->scale = res;
+ plan->size_x = sx;
+ plan->size_y = sy;
+ plan->origin_x = ox;
+ plan->origin_y = oy;
+
+ 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->occ_state_dyn = cell->occ_state;
+ if(cell->occ_state >= 0)
+ cell->occ_dist_dyn = cell->occ_dist = 0.0;
+ else
+ cell->occ_dist_dyn = cell->occ_dist = plan->max_radius;
+ cell->plan_cost = PLAN_MAX_COST;
cell->plan_next = NULL;
}
}
plan->waypoint_count = 0;
- return;
+
+ plan_compute_dist_kernel(plan);
+
+ plan_set_bounds(plan, 0, 0, plan->size_x - 1, plan->size_y - 1);
}
+
+// Reset the plan
+void plan_reset(plan_t *plan)
+{
+ int i, j;
+ plan_cell_t *cell;
+
+ for (j = plan->min_y; j <= plan->max_y; j++)
+ {
+ cell = plan->cells + PLAN_INDEX(plan,plan->min_x,j);
+ for (i = plan->min_x; i <= plan->max_x; i++, cell++)
+ {
+ cell->plan_cost = PLAN_MAX_COST;
+ cell->plan_next = NULL;
+ cell->mark = 0;
+ }
+ }
+ plan->waypoint_count = 0;
+}
+
void
plan_set_bounds(plan_t* plan, int min_x, int min_y, int max_x, int max_y)
{
- assert(min_x < plan->size_x);
- assert(min_y < plan->size_y);
- assert(max_x < plan->size_x);
- assert(max_y < plan->size_y);
+ min_x = MAX(0,min_x);
+ min_x = MIN(plan->size_x-1, min_x);
+ min_y = MAX(0,min_y);
+ min_y = MIN(plan->size_y-1, min_y);
+ max_x = MAX(0,max_x);
+ max_x = MIN(plan->size_x-1, max_x);
+ max_y = MAX(0,max_y);
+ max_y = MIN(plan->size_y-1, max_y);
+
assert(min_x <= max_x);
assert(min_y <= max_y);
@@ -124,6 +254,10 @@
plan->min_y = min_y;
plan->max_x = max_x;
plan->max_y = max_y;
+
+ //printf("new bounds: (%d,%d) -> (%d,%d)\n",
+ //plan->min_x, plan->min_y,
+ //plan->max_x, plan->max_y);
}
int
@@ -208,48 +342,48 @@
}
void
-plan_update_cspace_naive(plan_t* plan)
+plan_compute_cspace(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++)
+ ncell = cell + -plan->dist_kernel_width/2 + dj*plan->size_x;
+ for (di = -plan->dist_kernel_width/2;
+ di <= plan->dist_kernel_width/2;
+ di++, p++, ncell++)
{
- if (!PLAN_VALID_BOUNDS(plan, i + di, j + dj))
+ 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)
+ ncell->occ_dist_dyn = ncell->occ_dist = *p;
}
}
}
}
}
-#if 0
+#if 1
#include <gdk-pixbuf/gdk-pixbuf.h>
-void
+ void
draw_cspace(plan_t* plan, const char* fname)
{
GdkPixbuf* pixbuf;
@@ -295,123 +429,116 @@
plan->size_y,
plan->size_x * 3,
NULL, NULL);
-
+
gdk_pixbuf_save(pixbuf,fname,"png",&error,NULL);
gdk_pixbuf_unref(pixbuf);
free(pixels);
}
-#endif
-#if 0
-void
-plan_update_cspace_dp(plan_t* plan)
+ void
+draw_path(plan_t* plan, double lx, double ly, const char* fname)
{
+ GdkPixbuf* pixbuf;
+ GError* error = NULL;
+ guchar* pixels;
+ int p;
+ int paddr;
int i, j;
- int di, dj, dn;
- double r;
- plan_cell_t *cell, *ncell;
- //heap_t* Q;
- plan_cell_t** Q;
- int qhead, qtail;
+ plan_cell_t* cell;
- PLAYER_MSG0(2,"Generating C-space....");
+ pixels = (guchar*)malloc(sizeof(guchar)*plan->size_x*plan->size_y*3);
- // We'll look this far away from obstacles when updating cell costs
- dn = (int) ceil(plan->max_radius / plan->scale);
+ p=0;
+ for(j=plan->size_y-1;j>=0;j--)
+ {
+ for(i=0;i<plan->size_x;i++,p++)
+ {
+ paddr = p * 3;
+ if(plan->cells[PLAN_INDEX(plan,i,j)].occ_state == 1)
+ {
+ pixels[paddr] = 255;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 0;
+ }
+ else if(plan->cells[PLAN_INDEX(plan,i,j)].occ_dist < plan->max_radius)
+ {
+ pixels[paddr] = 0;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 255;
+ }
+ else
+ {
+ pixels[paddr] = 255;
+ pixels[paddr+1] = 255;
+ pixels[paddr+2] = 255;
+ }
+ /*
+ if((7*plan->cells[PLAN_INDEX(plan,i,j)].plan_cost) > 255)
+ {
+ pixels[paddr] = 0;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 255;
+ }
+ else
+ {
+ pixels[paddr] = 255 - 7*plan->cells[PLAN_INDEX(plan,i,j)].plan_cost;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 0;
+ }
+ */
+ }
+ }
- // 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);
+ for(i=0;i<plan->path_count;i++)
+ {
+ cell = plan->path[i];
+
+ paddr = 3*PLAN_INDEX(plan,cell->ci,plan->size_y - cell->cj - 1);
+ pixels[paddr] = 0;
+ pixels[paddr+1] = 255;
+ pixels[paddr+2] = 0;
+ }
- // Make space for the marks that we'll use.
- if(plan->marks_size != plan->size_x*plan->size_y)
+ for(i=0;i<plan->lpath_count;i++)
{
- plan->marks_size = plan->size_x*plan->size_y;
- plan->marks = (unsigned char*)realloc(plan->marks,
- sizeof(unsigned char*) *
- plan->marks_size);
+ cell = plan->lpath[i];
+
+ paddr = 3*PLAN_INDEX(plan,cell->ci,plan->size_y - cell->cj - 1);
+ pixels[paddr] = 255;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 255;
}
- 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(p=0;p<plan->waypoint_count;p++)
{
- for (i = 0; i < plan->size_x; i++)
+ cell = plan->waypoints[p];
+ for(j=-3;j<=3;j++)
{
- 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)
+ cj = cell->cj + j;
+ for(i=-3;i<=3;i++)
{
- //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);
- }
- }
- }
+ 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;
}
}
}
- //heap_free(Q);
+ */
+
+ pixbuf = gdk_pixbuf_new_from_data(pixels,
+ GDK_COLORSPACE_RGB,
+ 0,8,
+ plan->size_x,
+ plan->size_y,
+ plan->size_x * 3,
+ NULL, NULL);
+
+ gdk_pixbuf_save(pixbuf,fname,"png",&error,NULL);
+ gdk_pixbuf_unref(pixbuf);
+ free(pixels);
}
#endif
@@ -421,6 +548,7 @@
// If cachefile is non-NULL, then we try to read the c-space from that
// file. If that fails, then we construct the c-space as per normal and
// then write it out to cachefile.
+#if 0
void
plan_update_cspace(plan_t *plan, const char* cachefile)
{
@@ -457,6 +585,7 @@
draw_cspace(plan,"plan_cspace.png");
#endif
}
+#endif
#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
// Write the cspace occupancy distance values to a file, one per line.
Modified: code/player/branches/cmake/server/drivers/planner/wavefront/plan.h
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/plan.h
2008-04-18 07:54:24 UTC (rev 6359)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan.h
2008-04-18 08:07:57 UTC (rev 6360)
@@ -9,10 +9,15 @@
#ifndef PLAN_H
#define PLAN_H
+#include "heap.h"
+
#ifdef __cplusplus
extern "C" {
#endif
+#define PLAN_DEFAULT_HEAP_SIZE 1000
+#define PLAN_MAX_COST 1e9
+
// Description for a grid single cell
typedef struct _plan_cell_t
{
@@ -21,13 +26,18 @@
// Occupancy state (-1 = free, 0 = unknown, +1 = occ)
char occ_state;
+ char occ_state_dyn;
// Distance to the nearest occupied cell
float occ_dist;
+ float occ_dist_dyn;
// Distance (cost) to the goal
float plan_cost;
+ // Mark used in dynamic programming
+ char mark;
+
// The next cell in the plan
struct _plan_cell_t *plan_next;
@@ -61,14 +71,29 @@
// The grid data
plan_cell_t *cells;
- unsigned char* marks;
- size_t marks_size;
+
+ // Dynamic obstacle hitpoints
+ unsigned short* obs_pts;
+ size_t obs_pts_size;
+ size_t obs_pts_num;
+
+ // Distance penalty kernel, pre-computed in plan_compute_dist_kernel();
+ float* dist_kernel;
+ int dist_kernel_width;
+ float dist_kernel_3x3[9];
- // Queue of cells to update
- int queue_start, queue_len, queue_size;
- plan_cell_t **queue;
+ // Priority queue of cells to update
+ heap_t* heap;
- // Waypoints
+ // The global path
+ int path_count, path_size;
+ plan_cell_t **path;
+
+ // The local path (mainly for debugging)
+ int lpath_count, lpath_size;
+ plan_cell_t **lpath;
+
+ // Waypoints extracted from global path
int waypoint_count, waypoint_size;
plan_cell_t **waypoints;
} plan_t;
@@ -78,9 +103,15 @@
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);
+// Initialize the plan
+void plan_init(plan_t *plan,
+ double res, double sx, double sy, double ox, double oy);
+
// Reset the plan
void plan_reset(plan_t *plan);
@@ -97,11 +128,13 @@
int plan_check_inbounds(plan_t* plan, double x, double y);
// Construct the configuration space from the occupancy grid.
-void plan_update_cspace(plan_t *plan, const char* cachefile);
+//void plan_update_cspace(plan_t *plan, const char* cachefile);
+void plan_compute_cspace(plan_t *plan);
-// Generate the plan
-void plan_update_plan(plan_t *plan, double gx, double gy);
+int plan_do_global(plan_t *plan, double lx, double ly, double gx, double gy);
+int plan_do_local(plan_t *plan, double lx, double ly, double plan_halfwidth);
+
// Generate a path to the goal
void plan_update_waypoints(plan_t *plan, double px, double py);
Modified:
code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
2008-04-18 07:54:24 UTC (rev 6359)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/plan_plan.c
2008-04-18 08:07:57 UTC (rev 6360)
@@ -11,44 +11,147 @@
#include <stdlib.h>
#include <stdio.h>
#include <limits.h>
+#include <float.h>
#include "plan.h"
// Plan queue stuff
void plan_push(plan_t *plan, plan_cell_t *cell);
plan_cell_t *plan_pop(plan_t *plan);
+int _plan_update_plan(plan_t *plan, double lx, double ly, double gx, double
gy);
+int _plan_find_local_goal(plan_t *plan, double* gx, double* gy, double lx,
double ly);
+int
+plan_do_global(plan_t *plan, double lx, double ly, double gx, double gy)
+{
+ plan_cell_t* cell;
+ int li, lj;
+
+ // 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;
+ }
+
+ return(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;
+
+ // 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);
+
+ 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;
+ }
+ return(0);
+}
+
+
// Generate the plan
-void plan_update_plan(plan_t *plan, double gx, double gy)
+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;
+ int reached_start=0;
- // Reset the grid
- for (nj = 0; nj < plan->size_y; nj++)
- {
- for (ni = 0; ni < plan->size_x; ni++)
- {
- cell = plan->cells + PLAN_INDEX(plan, ni, nj);
- cell->plan_cost = 1e6;
- cell->plan_next = NULL;
- }
- }
-
// Reset the queue
- plan->queue_start = 0;
- plan->queue_len = 0;
+ 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);
+
+ // 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, ni, nj))
- return;
+ if(!PLAN_VALID_BOUNDS(plan, li, lj))
+ {
+ puts("start out of bounds");
+ return(-1);
+ }
- cell = plan->cells + PLAN_INDEX(plan, ni, nj);
+ cell = plan->cells + PLAN_INDEX(plan, gi, gj);
cell->plan_cost = 0;
plan_push(plan, cell);
@@ -63,54 +166,141 @@
//printf("pop %d %d %f\n", cell->ci, cell->cj, cell->plan_cost);
+ float* p = plan->dist_kernel_3x3;
for (dj = -1; dj <= +1; dj++)
{
- for (di = -1; di <= +1; di++)
+ ncell = plan->cells + PLAN_INDEX(plan,oi-1,oj+dj);
+ for (di = -1; di <= +1; di++, p++, ncell++)
{
- if (di == 0 && dj == 0)
+ if (!di && !dj)
continue;
+ //if (di && dj)
+ //continue;
ni = oi + di;
nj = oj + dj;
if (!PLAN_VALID_BOUNDS(plan, ni, nj))
continue;
- ncell = plan->cells + PLAN_INDEX(plan, ni, nj);
- if (ncell->occ_dist < plan->abs_min_radius)
+ if(ncell->mark)
continue;
- cost = cell->plan_cost + plan->scale;
+ if (ncell->occ_dist_dyn < plan->abs_min_radius)
+ continue;
- if (ncell->occ_dist < plan->max_radius)
- cost += plan->dist_penalty * (plan->max_radius - ncell->occ_dist);
+ cost = cell->plan_cost + *p;
- if (cost < ncell->plan_cost)
+ 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;
+
+ // Are we done?
+ if((ncell->ci == li) && (ncell->cj == lj))
+ reached_start=1;
+
plan_push(plan, ncell);
}
}
}
}
- return;
+ /*
+ if(!reached_start)
+ {
+ puts("never found start");
+ return(-1);
+ }
+ else
+ */
+ return(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);
+
+ // 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);
+}
+
// Push a plan location onto the queue
void plan_push(plan_t *plan, plan_cell_t *cell)
{
- int i;
+ // Substract from max cost because the heap is set up to return the max
+ // element. This could of course be changed.
+ assert(PLAN_MAX_COST-cell->plan_cost > 0);
+ cell->mark = 1;
+ heap_insert(plan->heap, PLAN_MAX_COST - cell->plan_cost, cell);
- // HACK: should resize the queue dynamically (tricky for circular queue)
- assert(plan->queue_len < plan->queue_size);
-
- i = (plan->queue_start + plan->queue_len) % plan->queue_size;
- plan->queue[i] = cell;
- plan->queue_len++;
-
return;
}
@@ -118,16 +308,9 @@
// Pop a plan location from the queue
plan_cell_t *plan_pop(plan_t *plan)
{
- int i;
- plan_cell_t *cell;
-
- if (plan->queue_len == 0)
- return NULL;
- i = plan->queue_start % plan->queue_size;
- cell = plan->queue[i];
- plan->queue_start++;
- plan->queue_len--;
-
- return cell;
+ if(heap_empty(plan->heap))
+ return(NULL);
+ else
+ return(heap_extract_max(plan->heap));
}
Copied: code/player/branches/cmake/server/drivers/planner/wavefront/test.c
(from rev 6287, code/player/trunk/server/drivers/planner/wavefront/test.c)
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/test.c
(rev 0)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/test.c
2008-04-18 08:07:57 UTC (rev 6360)
@@ -0,0 +1,225 @@
+#include <stdio.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <sys/time.h>
+
+#include <gdk-pixbuf/gdk-pixbuf.h>
+
+#include "plan.h"
+
+#define USAGE "USAGE: test <map.png> <res> <lx> <ly> <gx> <gy>"
+
+// compute linear index for given map coords
+#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
+
+void draw_cspace(plan_t* plan, const char* fname);
+void draw_path(plan_t* plan, double lx, double ly, const char* fname);
+
+double get_time(void);
+
+int read_map_from_image(int* size_x, int* size_y, char** mapdata,
+ const char* fname, int negate);
+
+int
+main(int argc, char** argv)
+{
+ char* fname;
+ double res;
+ double lx,ly,gx,gy;
+
+
+ char* mapdata;
+ int sx, sy;
+ int i, j;
+
+ plan_t* plan;
+ double robot_radius=0.25;
+ double safety_dist=0.2;
+ double max_radius=1.0;
+ double dist_penalty=1.0;;
+ double plan_halfwidth = 5.0;
+
+ double t_c0, t_c1, t_p0, t_p1, t_w0, t_w1;
+
+ if(argc < 7)
+ {
+ puts(USAGE);
+ exit(-1);
+ }
+
+ fname = argv[1];
+ res = atof(argv[2]);
+ lx = atof(argv[3]);
+ ly = atof(argv[4]);
+ gx = atof(argv[5]);
+ gy = atof(argv[6]);
+
+ assert(read_map_from_image(&sx, &sy, &mapdata, fname, 0) == 0);
+
+ assert((plan = plan_alloc(robot_radius+safety_dist,
+ robot_radius+safety_dist,
+ max_radius,
+ dist_penalty)));
+
+ // allocate space for map cells
+ assert(plan->cells == NULL);
+ assert((plan->cells =
+ (plan_cell_t*)realloc(plan->cells,
+ (sx * sy * sizeof(plan_cell_t)))));
+
+ // Copy over obstacle information from the image data that we read
+ for(j=0;j<sy;j++)
+ {
+ for(i=0;i<sx;i++)
+ {
+ plan->cells[i+j*sx].occ_state = mapdata[MAP_IDX(sx,i,j)];
+ }
+ }
+ free(mapdata);
+
+ // Do initialization
+ plan_init(plan, res, sx, sy, 0.0, 0.0);
+
+ t_c0 = get_time();
+ plan_compute_cspace(plan);
+ t_c1 = get_time();
+
+ draw_cspace(plan,"cspace.png");
+
+ printf("cspace: %.6lf\n", t_c1-t_c0);
+
+ // compute costs to the new goal
+ t_p0 = get_time();
+ if(plan_do_global(plan, lx, ly, gx, gy) < 0)
+ puts("no global path");
+ t_p1 = get_time();
+
+ plan_update_waypoints(plan, lx, ly);
+
+ printf("gplan : %.6lf\n", t_p1-t_p0);
+ for(i=0;i<plan->waypoint_count;i++)
+ {
+ double wx, wy;
+ plan_convert_waypoint(plan, plan->waypoints[i], &wx, &wy);
+ printf("%d: (%d,%d) : (%.3lf,%.3lf)\n",
+ i, plan->waypoints[i]->ci, plan->waypoints[i]->cj, wx, wy);
+ }
+
+ for(i=0;i<1;i++)
+ {
+ // compute costs to the new goal
+ t_p0 = get_time();
+ if(plan_do_local(plan, lx, ly, plan_halfwidth) < 0)
+ puts("no local path");
+ t_p1 = get_time();
+
+ printf("lplan : %.6lf\n", t_p1-t_p0);
+
+ // compute a path to the goal from the current position
+ t_w0 = get_time();
+ plan_update_waypoints(plan, lx, ly);
+ t_w1 = get_time();
+
+ draw_path(plan,lx,ly,"plan.png");
+
+ printf("waypnt: %.6lf\n", t_w1-t_w0);
+ puts("");
+ }
+
+ if(plan->waypoint_count == 0)
+ {
+ puts("no waypoints");
+ }
+ else
+ {
+ for(i=0;i<plan->waypoint_count;i++)
+ {
+ double wx, wy;
+ plan_convert_waypoint(plan, plan->waypoints[i], &wx, &wy);
+ printf("%d: (%d,%d) : (%.3lf,%.3lf)\n",
+ i, plan->waypoints[i]->ci, plan->waypoints[i]->cj, wx, wy);
+ }
+ }
+
+ plan_free(plan);
+
+ return(0);
+}
+
+int
+read_map_from_image(int* size_x, int* size_y, char** mapdata,
+ const char* fname, int negate)
+{
+ GdkPixbuf* pixbuf;
+ guchar* pixels;
+ guchar* p;
+ int rowstride, n_channels, bps;
+ GError* error = NULL;
+ int i,j,k;
+ double occ;
+ int color_sum;
+ double color_avg;
+
+ // Initialize glib
+ g_type_init();
+
+ printf("MapFile loading image file: %s...", fname);
+ fflush(stdout);
+
+ // Read the image
+ if(!(pixbuf = gdk_pixbuf_new_from_file(fname, &error)))
+ {
+ printf("failed to open image file %s", fname);
+ return(-1);
+ }
+
+ *size_x = gdk_pixbuf_get_width(pixbuf);
+ *size_y = gdk_pixbuf_get_height(pixbuf);
+
+ assert(*mapdata = (char*)malloc(sizeof(char) * (*size_x) * (*size_y)));
+
+ rowstride = gdk_pixbuf_get_rowstride(pixbuf);
+ bps = gdk_pixbuf_get_bits_per_sample(pixbuf)/8;
+ n_channels = gdk_pixbuf_get_n_channels(pixbuf);
+ if(gdk_pixbuf_get_has_alpha(pixbuf))
+ n_channels++;
+
+ // Read data
+ pixels = gdk_pixbuf_get_pixels(pixbuf);
+ for(j = 0; j < *size_y; j++)
+ {
+ for (i = 0; i < *size_x; i++)
+ {
+ p = pixels + j*rowstride + i*n_channels*bps;
+ color_sum = 0;
+ for(k=0;k<n_channels;k++)
+ color_sum += *(p + (k * bps));
+ color_avg = color_sum / (double)n_channels;
+
+ if(negate)
+ occ = color_avg / 255.0;
+ else
+ occ = (255 - color_avg) / 255.0;
+ if(occ > 0.95)
+ (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = +1;
+ else if(occ < 0.1)
+ (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = -1;
+ else
+ (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = 0;
+ }
+ }
+
+ gdk_pixbuf_unref(pixbuf);
+
+ puts("Done.");
+ printf("MapFile read a %d X %d map\n", *size_x, *size_y);
+ return(0);
+}
+
+double
+get_time(void)
+{
+ struct timeval curr;
+ gettimeofday(&curr,NULL);
+ return(curr.tv_sec + curr.tv_usec / 1e6);
+}
Modified:
code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
===================================================================
--- code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
2008-04-18 07:54:24 UTC (rev 6359)
+++ code/player/branches/cmake/server/drivers/planner/wavefront/wavefront.cc
2008-04-18 08:07:57 UTC (rev 6360)
@@ -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;
@@ -766,10 +767,17 @@
}
#endif
- // compute costs to the new goal
- plan_update_plan(this->plan, this->target_x, this->target_y);
+ // compute costs to the new goal. Try local plan first
+ if(plan_do_local(this->plan, this->localize_x, this->localize_y, 5.0) <
0)
+ {
+ puts("Wavefront: local plan failed");
- // compute a path to the goal from the current position
+ if(plan_do_global(this->plan, this->localize_x, this->localize_x,
+ this->target_x, this->target_y) < 0)
+ puts("Wavefront: global plan failed");
+ }
+
+ // extract waypoints along the path to the goal from the current position
plan_update_waypoints(this->plan, this->localize_x, this->localize_y);
#if 0
@@ -1124,6 +1132,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 +1144,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;
@@ -1180,7 +1190,8 @@
if(this->GetMap(false) < 0)
return(-1);
- plan_update_cspace(this->plan,this->cspace_fname);
+ //plan_update_cspace(this->plan,this->cspace_fname);
+ plan_compute_cspace(this->plan);
this->have_map = true;
this->new_map = true;
@@ -1260,7 +1271,8 @@
// Now get the map data, possibly in separate tiles.
if(this->GetMap(true) < 0) return -1;
- plan_update_cspace(this->plan,this->cspace_fname);
+ //plan_update_cspace(this->plan,this->cspace_fname);
+ plan_compute_cspace(this->plan);
this->have_map = true;
this->new_map = true;
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
Don't miss this year's exciting event. There's still time to save $100.
Use priority code J8TL2D2.
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
