Revision: 4479
http://playerstage.svn.sourceforge.net/playerstage/?rev=4479&view=rev
Author: gerkey
Date: 2008-04-04 17:31:33 -0700 (Fri, 04 Apr 2008)
Log Message:
-----------
added heap for priority queue, fixed cost propagation
Modified Paths:
--------------
code/player/trunk/server/drivers/planner/wavefront/Makefile.am
code/player/trunk/server/drivers/planner/wavefront/Makefile.test
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
Added Paths:
-----------
code/player/trunk/server/drivers/planner/wavefront/heap.c
code/player/trunk/server/drivers/planner/wavefront/heap.h
Modified: code/player/trunk/server/drivers/planner/wavefront/Makefile.am
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/Makefile.am
2008-04-04 19:59:14 UTC (rev 4478)
+++ code/player/trunk/server/drivers/planner/wavefront/Makefile.am
2008-04-05 00:31:33 UTC (rev 4479)
@@ -6,5 +6,4 @@
AM_CPPFLAGS = -Wall -g -I$(top_srcdir) `pkg-config --cflags gdk-pixbuf-2.0`
libwavefront_la_SOURCES = plan.c plan_plan.c plan_waypoint.c plan.h \
- wavefront.cc
-#heap.h heap.c
+ wavefront.cc heap.h heap.c
Modified: code/player/trunk/server/drivers/planner/wavefront/Makefile.test
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/Makefile.test
2008-04-04 19:59:14 UTC (rev 4478)
+++ code/player/trunk/server/drivers/planner/wavefront/Makefile.test
2008-04-05 00:31:33 UTC (rev 4479)
@@ -1,9 +1,9 @@
CFLAGS = -I. -I../../../.. -Wall -Werror -O2 -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
+SRCS = test.c plan.c plan_plan.c plan_waypoint.c heap.c
-test: $(SRCS) plan.h
+test: $(SRCS) plan.h heap.h
gcc $(CFLAGS) -o $@ $(SRCS) $(LIBS)
clean:
Added: code/player/trunk/server/drivers/planner/wavefront/heap.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/heap.c
(rev 0)
+++ code/player/trunk/server/drivers/planner/wavefront/heap.c 2008-04-05
00:31:33 UTC (rev 4479)
@@ -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]);
+}
Added: code/player/trunk/server/drivers/planner/wavefront/heap.h
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/heap.h
(rev 0)
+++ code/player/trunk/server/drivers/planner/wavefront/heap.h 2008-04-05
00:31:33 UTC (rev 4479)
@@ -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/trunk/server/drivers/planner/wavefront/plan.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-04
19:59:14 UTC (rev 4478)
+++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-05
00:31:33 UTC (rev 4479)
@@ -63,10 +63,12 @@
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->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->waypoint_size = 100;
@@ -81,7 +83,7 @@
{
if (plan->cells)
free(plan->cells);
- free(plan->queue);
+ heap_free(plan->heap);
free(plan->waypoints);
free(plan);
@@ -218,7 +220,7 @@
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++)
@@ -238,7 +240,7 @@
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;
@@ -248,10 +250,10 @@
}
}
-#if 0
+#if 1
#include <gdk-pixbuf/gdk-pixbuf.h>
-void
+ void
draw_cspace(plan_t* plan, const char* fname)
{
GdkPixbuf* pixbuf;
@@ -297,6 +299,105 @@
plan->size_y,
plan->size_x * 3,
NULL, NULL);
+
+ gdk_pixbuf_save(pixbuf,fname,"png",&error,NULL);
+ gdk_pixbuf_unref(pixbuf);
+ free(pixels);
+}
+
+ 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 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;
+ 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;
+ }
+ */
+ }
+ }
+
+ 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;
+ }
+
+ for(p=0;p<plan->waypoint_count;p++)
+ {
+ cell = plan->waypoints[p];
+ for(j=-3;j<=3;j++)
+ {
+ 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;
+ }
+ }
+ }
+
+ 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);
Modified: code/player/trunk/server/drivers/planner/wavefront/plan.h
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-04
19:59:14 UTC (rev 4478)
+++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-05
00:31:33 UTC (rev 4479)
@@ -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
{
@@ -65,8 +70,9 @@
size_t marks_size;
// Queue of cells to update
- int queue_start, queue_len, queue_size;
- plan_cell_t **queue;
+ //int queue_start, queue_len, queue_size;
+ //plan_cell_t **queue;
+ heap_t* heap;
// Waypoints
int waypoint_count, waypoint_size;
Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c
2008-04-04 19:59:14 UTC (rev 4478)
+++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c
2008-04-05 00:31:33 UTC (rev 4479)
@@ -32,14 +32,15 @@
for (ni = 0; ni < plan->size_x; ni++)
{
cell = plan->cells + PLAN_INDEX(plan, ni, nj);
- cell->plan_cost = 1e6;
+ cell->plan_cost = PLAN_MAX_COST;
cell->plan_next = NULL;
}
}
// Reset the queue
- plan->queue_start = 0;
- plan->queue_len = 0;
+ //plan->queue_start = 0;
+ //plan->queue_len = 0;
+ heap_reset(plan->heap);
// Initialize the goal cell
ni = PLAN_GXWX(plan, gx);
@@ -67,8 +68,10 @@
{
for (di = -1; di <= +1; di++)
{
- if (di == 0 && dj == 0)
+ if (!di && !dj)
continue;
+ //if (di && dj)
+ //continue;
ni = oi + di;
nj = oj + dj;
@@ -80,7 +83,10 @@
if (ncell->occ_dist < plan->abs_min_radius)
continue;
- cost = cell->plan_cost + plan->scale;
+ if(di && dj)
+ cost = cell->plan_cost + plan->scale * M_SQRT2;
+ else
+ cost = cell->plan_cost + plan->scale;
if (ncell->occ_dist < plan->max_radius)
cost += plan->dist_penalty * (plan->max_radius - ncell->occ_dist);
@@ -102,6 +108,7 @@
// Push a plan location onto the queue
void plan_push(plan_t *plan, plan_cell_t *cell)
{
+ /*
int i;
// HACK: should resize the queue dynamically (tricky for circular queue)
@@ -110,7 +117,12 @@
i = (plan->queue_start + plan->queue_len) % plan->queue_size;
plan->queue[i] = cell;
plan->queue_len++;
+ */
+ // Substract from max cost because the heap is set up to return the max
+ // element. This could of course be changed.
+ heap_insert(plan->heap, PLAN_MAX_COST - cell->plan_cost, cell);
+
return;
}
@@ -118,6 +130,7 @@
// Pop a plan location from the queue
plan_cell_t *plan_pop(plan_t *plan)
{
+ /*
int i;
plan_cell_t *cell;
@@ -128,6 +141,10 @@
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));
}
Modified: code/player/trunk/server/drivers/planner/wavefront/test.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-04
19:59:14 UTC (rev 4478)
+++ code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-05
00:31:33 UTC (rev 4479)
@@ -12,6 +12,9 @@
// 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,
@@ -35,7 +38,7 @@
double max_radius=1.0;
double dist_penalty=1.0;;
- double t0, t1, t2, t3;
+ double t_c0, t_c1, t_p0, t_p1, t_w0, t_w1;
if(argc < 7)
{
@@ -86,22 +89,28 @@
free(mapdata);
- t0 = get_time();
+ t_c0 = get_time();
plan_update_cspace(plan,NULL);
- t1 = get_time();
+ t_c1 = get_time();
+ draw_cspace(plan,"cspace.png");
+
// compute costs to the new goal
+ t_p0 = get_time();
plan_update_plan(plan, gx, gy);
- t2 = get_time();
+ t_p1 = get_time();
// compute a path to the goal from the current position
+ t_w0 = get_time();
plan_update_waypoints(plan, lx, ly);
- t3 = get_time();
+ t_w1 = get_time();
- printf("cspace: %.6lf\n", t1-t0);
- printf("update: %.6lf\n", t2-t1);
- printf("waypnt: %.6lf\n", t3-t2);
+ draw_path(plan,lx,ly,"plan.png");
+ printf("cspace: %.6lf\n", t_c1-t_c0);
+ printf("plan : %.6lf\n", t_p1-t_p0);
+ printf("waypnt: %.6lf\n", t_w1-t_w0);
+
if(plan->waypoint_count == 0)
{
puts("no path");
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
