From: <ge...@us...> - 2008-04-04 12:59:08
|
Revision: 4478 http://playerstage.svn.sourceforge.net/playerstage/?rev=4478&view=rev Author: gerkey Date: 2008-04-04 12:59:14 -0700 (Fri, 04 Apr 2008) Log Message: ----------- added standalone test to use in profiling planner Added Paths: ----------- code/player/trunk/server/drivers/planner/wavefront/Makefile.test code/player/trunk/server/drivers/planner/wavefront/test.c Added: code/player/trunk/server/drivers/planner/wavefront/Makefile.test =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/Makefile.test (rev 0) +++ code/player/trunk/server/drivers/planner/wavefront/Makefile.test 2008-04-04 19:59:14 UTC (rev 4478) @@ -0,0 +1,10 @@ +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 + +test: $(SRCS) plan.h + gcc $(CFLAGS) -o $@ $(SRCS) $(LIBS) + +clean: + rm -f test Added: code/player/trunk/server/drivers/planner/wavefront/test.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/test.c (rev 0) +++ code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-04 19:59:14 UTC (rev 4478) @@ -0,0 +1,201 @@ +#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)) + +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 t0, t1, t2, t3; + + 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))); + + plan->scale = res; + plan->size_x = sx; + plan->size_y = sy; + plan->origin_x = 0.0; + plan->origin_y = 0.0; + + // Set the bounds to search the whole grid. + plan_set_bounds(plan, 0, 0, plan->size_x - 1, plan->size_y - 1); + + // allocate space for map cells + assert(plan->cells == NULL); + assert((plan->cells = + (plan_cell_t*)realloc(plan->cells, + (sx * sy * sizeof(plan_cell_t))))); + plan_reset(plan); + + for(j=0;j<sy;j++) + { + for(i=0;i<sx;i++) + { + plan->cells[PLAN_INDEX(plan,i,j)].occ_dist = max_radius; + if((plan->cells[PLAN_INDEX(plan,i,j)].occ_state = + mapdata[MAP_IDX(sx,i,j)]) >= 0) + plan->cells[PLAN_INDEX(plan,i,j)].occ_dist = 0; + } + } + + free(mapdata); + + t0 = get_time(); + plan_update_cspace(plan,NULL); + t1 = get_time(); + + // compute costs to the new goal + plan_update_plan(plan, gx, gy); + t2 = get_time(); + + // compute a path to the goal from the current position + plan_update_waypoints(plan, lx, ly); + t3 = get_time(); + + printf("cspace: %.6lf\n", t1-t0); + printf("update: %.6lf\n", t2-t1); + printf("waypnt: %.6lf\n", t3-t2); + + if(plan->waypoint_count == 0) + { + puts("no path"); + } + 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); +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-04-04 17:31:26
|
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 ge...@wi... + * + * + * 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 ge...@wi... + * + * + * 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. |
From: <ge...@us...> - 2008-04-04 18:55:11
|
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. |
From: <ge...@us...> - 2008-04-05 10:00:36
|
Revision: 4481 http://playerstage.svn.sourceforge.net/playerstage/?rev=4481&view=rev Author: gerkey Date: 2008-04-05 10:00:42 -0700 (Sat, 05 Apr 2008) Log Message: ----------- more cspace speedups, commented out debug graphics code Modified Paths: -------------- code/player/trunk/server/drivers/planner/wavefront/plan.c code/player/trunk/server/drivers/planner/wavefront/test.c Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-05 01:55:18 UTC (rev 4480) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-05 17:00:42 UTC (rev 4481) @@ -105,14 +105,6 @@ 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(""); - } } @@ -270,24 +262,22 @@ if (cell->occ_state < 0) continue; - cell->occ_dist = FLT_MAX; + cell->occ_dist = plan->max_radius; p = plan->dist_kernel; for (dj = -plan->dist_kernel_width/2; dj <= plan->dist_kernel_width/2; dj++) { - 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++) + 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)) continue; - //if(*p < ncell->occ_dist) - if(0.1 < ncell->occ_dist) + if(*p < ncell->occ_dist) ncell->occ_dist = *p; } } @@ -295,7 +285,7 @@ } } -#if 1 +#if 0 #include <gdk-pixbuf/gdk-pixbuf.h> void Modified: code/player/trunk/server/drivers/planner/wavefront/test.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-05 01:55:18 UTC (rev 4480) +++ code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-05 17:00:42 UTC (rev 4481) @@ -90,6 +90,8 @@ free(mapdata); + plan_set_bbox(plan, 25.0, 0.0, lx, ly, gx, gy); + t_c0 = get_time(); plan_update_cspace(plan,NULL); t_c1 = get_time(); @@ -111,7 +113,9 @@ printf("cspace: %.6lf\n", t_c1-t_c0); printf("plan : %.6lf\n", t_p1-t_p0); printf("waypnt: %.6lf\n", t_w1-t_w0); + printf("total : %.6lf\n", (t_c1-t_c0)+(t_p1-t_p0)+(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. |
From: <ge...@us...> - 2008-04-07 18:29:16
|
Revision: 6284 http://playerstage.svn.sourceforge.net/playerstage/?rev=6284&view=rev Author: gerkey Date: 2008-04-07 18:29:22 -0700 (Mon, 07 Apr 2008) Log Message: ----------- adding more features to planner Modified Paths: -------------- 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 code/player/trunk/server/drivers/planner/wavefront/wavefront.cc Modified: code/player/trunk/server/drivers/planner/wavefront/Makefile.test =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/Makefile.test 2008-04-08 00:58:08 UTC (rev 6283) +++ code/player/trunk/server/drivers/planner/wavefront/Makefile.test 2008-04-08 01:29:22 UTC (rev 6284) @@ -1,4 +1,5 @@ -CFLAGS = -I. -I../../../.. -Wall -Werror -O2 -g `pkg-config --cflags gdk-pixbuf-2.0` +#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 Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-08 00:58:08 UTC (rev 6283) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-08 01:29:22 UTC (rev 6284) @@ -48,31 +48,18 @@ 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_kernel_width = 0; - plan->dist_kernel = NULL; 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->waypoint_size = 100; plan->waypoints = calloc(plan->waypoint_size, sizeof(plan->waypoints[0])); @@ -80,6 +67,69 @@ } 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; @@ -95,16 +145,22 @@ assert(plan->dist_kernel); p = plan->dist_kernel; - for(j=-plan->dist_kernel_width/2;j<plan->dist_kernel_width/2;j++) + 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++) + 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; } } + // 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; + } + } } @@ -122,13 +178,19 @@ 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++) { @@ -136,23 +198,53 @@ { cell->ci = i; cell->cj = j; - cell->occ_state = 0; - cell->occ_dist = plan->max_radius; + 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; + + cell = plan->cells; + for (j = plan->min_y; j <= plan->max_y; j++) + { + for (i = plan->min_x; i <= plan->max_x; i++, cell++) + { + cell->plan_cost = PLAN_MAX_COST; + cell->plan_next = NULL; + } + } + 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); @@ -244,7 +336,7 @@ } void -plan_update_cspace_naive(plan_t* plan) +plan_compute_cspace(plan_t* plan) { int i, j; int di, dj; @@ -253,7 +345,6 @@ PLAYER_MSG0(2,"Generating C-space...."); - for (j = plan->min_y; j <= plan->max_y; j++) { cell = plan->cells + PLAN_INDEX(plan, 0, j); @@ -262,8 +353,6 @@ if (cell->occ_state < 0) continue; - cell->occ_dist = plan->max_radius; - p = plan->dist_kernel; for (dj = -plan->dist_kernel_width/2; dj <= plan->dist_kernel_width/2; @@ -274,18 +363,18 @@ 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; if(*p < ncell->occ_dist) - ncell->occ_dist = *p; + ncell->occ_dist_dyn = ncell->occ_dist = *p; } } } } } -#if 0 +#if 1 #include <gdk-pixbuf/gdk-pixbuf.h> void @@ -449,6 +538,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) { @@ -485,6 +575,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/trunk/server/drivers/planner/wavefront/plan.h =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-08 00:58:08 UTC (rev 6283) +++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-08 01:29:22 UTC (rev 6284) @@ -26,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; @@ -67,17 +72,24 @@ // The grid data plan_cell_t *cells; + // 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[3][3]; + 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; + + // Waypoints extracted from global path int waypoint_count, waypoint_size; plan_cell_t **waypoints; } plan_t; @@ -92,6 +104,10 @@ // 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); @@ -108,11 +124,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 lx, double ly, 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/trunk/server/drivers/planner/wavefront/plan_plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-08 00:58:08 UTC (rev 6283) +++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-08 01:29:22 UTC (rev 6284) @@ -17,11 +17,74 @@ // 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; + + // Set bounds as directed + plan_set_bounds(plan, + lx - plan_halfwidth, ly - plan_halfwidth, + lx + plan_halfwidth, ly + plan_halfwidth); + + // 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) + return(-1); + + return(_plan_update_plan(plan, lx, ly, gx, gy)); +} + + // Generate the plan -void -plan_update_plan(plan_t *plan, double lx, double ly, 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; @@ -36,12 +99,11 @@ { cell->plan_cost = PLAN_MAX_COST; cell->plan_next = NULL; + cell->mark = 0; } } // Reset the queue - //plan->queue_start = 0; - //plan->queue_len = 0; heap_reset(plan->heap); // Initialize the goal cell @@ -49,14 +111,14 @@ gj = PLAN_GYWY(plan, gy); if (!PLAN_VALID_BOUNDS(plan, gi, gj)) - return; + return(-1); // Initialize the start cell li = PLAN_GXWX(plan, lx); lj = PLAN_GYWY(plan, ly); if (!PLAN_VALID_BOUNDS(plan, li, lj)) - return; + return(-1); cell = plan->cells + PLAN_INDEX(plan, gi, gj); cell->plan_cost = 0; @@ -73,9 +135,11 @@ //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 && !dj) continue; @@ -87,57 +151,50 @@ 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; - /* - if(di && dj) - cost = cell->plan_cost + plan->scale * M_SQRT2; - else - cost = cell->plan_cost + plan->scale; - */ + if (ncell->occ_dist_dyn < plan->abs_min_radius) + continue; - cost = cell->plan_cost + plan->dist_kernel_3x3[di+1][dj+1]; + cost = cell->plan_cost + *p; - if (ncell->occ_dist < plan->max_radius) - cost += plan->dist_penalty * (plan->max_radius - ncell->occ_dist); + if(ncell->occ_dist_dyn < plan->max_radius) + cost += plan->dist_penalty * (plan->max_radius - ncell->occ_dist_dyn); - if (cost < ncell->plan_cost) + if(cost < ncell->plan_cost) { ncell->plan_cost = cost; ncell->plan_next = cell; // Are we done? if((ncell->ci == li) && (ncell->cj == lj)) - return; + return(0); + plan_push(plan, ncell); } } } } - return; + return(-1); } +int +_plan_find_local_goal(plan_t *plan, double* gx, double* gy, + double lx, double ly) +{ + return(0); +} // 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) - assert(plan->queue_len < plan->queue_size); - - 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. + assert(PLAN_MAX_COST-cell->plan_cost > 0); + cell->mark = 1; heap_insert(plan->heap, PLAN_MAX_COST - cell->plan_cost, cell); return; @@ -147,19 +204,7 @@ // 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--; - */ - if(heap_empty(plan->heap)) return(NULL); else Modified: code/player/trunk/server/drivers/planner/wavefront/test.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-08 00:58:08 UTC (rev 6283) +++ code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-08 01:29:22 UTC (rev 6284) @@ -60,61 +60,51 @@ max_radius, dist_penalty))); - plan->scale = res; - plan_compute_dist_kernel(plan); - plan->size_x = sx; - plan->size_y = sy; - plan->origin_x = 0.0; - plan->origin_y = 0.0; - - // Set the bounds to search the whole grid. - plan_set_bounds(plan, 0, 0, plan->size_x - 1, plan->size_y - 1); - // allocate space for map cells assert(plan->cells == NULL); assert((plan->cells = (plan_cell_t*)realloc(plan->cells, (sx * sy * sizeof(plan_cell_t))))); - plan_reset(plan); - + + // 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[PLAN_INDEX(plan,i,j)].occ_dist = max_radius; - if((plan->cells[PLAN_INDEX(plan,i,j)].occ_state = - mapdata[MAP_IDX(sx,i,j)]) >= 0) - plan->cells[PLAN_INDEX(plan,i,j)].occ_dist = 0; + 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); - plan_set_bbox(plan, 25.0, 0.0, lx, ly, gx, gy); - t_c0 = get_time(); - plan_update_cspace(plan,NULL); + plan_compute_cspace(plan); t_c1 = get_time(); draw_cspace(plan,"cspace.png"); - // compute costs to the new goal - t_p0 = get_time(); - plan_update_plan(plan, lx, ly, gx, gy); - t_p1 = get_time(); + printf("cspace: %.6lf\n", t_c1-t_c0); - // compute a path to the goal from the current position - t_w0 = get_time(); - plan_update_waypoints(plan, lx, ly); - t_w1 = get_time(); + for(i=0;i<10;i++) + { + // compute costs to the new goal + t_p0 = get_time(); + plan_do_global(plan, lx, ly, gx, gy); + t_p1 = get_time(); - draw_path(plan,lx,ly,"plan.png"); + // compute a path to the goal from the current position + t_w0 = get_time(); + plan_update_waypoints(plan, lx, ly); + t_w1 = get_time(); - printf("cspace: %.6lf\n", t_c1-t_c0); - printf("plan : %.6lf\n", t_p1-t_p0); - printf("waypnt: %.6lf\n", t_w1-t_w0); - printf("total : %.6lf\n", (t_c1-t_c0)+(t_p1-t_p0)+(t_w1-t_w0)); + draw_path(plan,lx,ly,"plan.png"); + printf("plan : %.6lf\n", t_p1-t_p0); + printf("waypnt: %.6lf\n", t_w1-t_w0); + puts(""); + } if(plan->waypoint_count == 0) { Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-08 00:58:08 UTC (rev 6283) +++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-08 01:29:22 UTC (rev 6284) @@ -768,8 +768,8 @@ #endif // compute costs to the new goal - plan_update_plan(this->plan, this->localize_x, this->localize_x, - this->target_x, this->target_y); + plan_do_global(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); @@ -1184,7 +1184,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; @@ -1264,7 +1265,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. |
From: <ge...@us...> - 2008-04-08 11:51:42
|
Revision: 6285 http://playerstage.svn.sourceforge.net/playerstage/?rev=6285&view=rev Author: gerkey Date: 2008-04-08 11:51:43 -0700 (Tue, 08 Apr 2008) Log Message: ----------- working on windowed local planning 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 Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-08 01:29:22 UTC (rev 6284) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-08 18:51:43 UTC (rev 6285) @@ -60,6 +60,9 @@ 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])); @@ -228,6 +231,7 @@ { cell->plan_cost = PLAN_MAX_COST; cell->plan_next = NULL; + cell->mark = 0; } } plan->waypoint_count = 0; @@ -438,7 +442,6 @@ int p; int paddr; int i, j; - int ci, cj; plan_cell_t* cell; pixels = (guchar*)malloc(sizeof(guchar)*plan->size_x*plan->size_y*3); @@ -484,39 +487,44 @@ } } - ci = PLAN_GXWX(plan, lx); - cj = PLAN_GYWY(plan, ly); + 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; + } - if(PLAN_VALID_BOUNDS(plan,ci,cj)) + for(i=0;i<plan->lpath_count;i++) { - 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 = 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; + } - cell = cell->plan_next; - } - - for(p=0;p<plan->waypoint_count;p++) + /* + for(p=0;p<plan->waypoint_count;p++) + { + cell = plan->waypoints[p]; + for(j=-3;j<=3;j++) { - cell = plan->waypoints[p]; - for(j=-3;j<=3;j++) + cj = cell->cj + j; + for(i=-3;i<=3;i++) { - 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; - } + 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, Modified: code/player/trunk/server/drivers/planner/wavefront/plan.h =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-08 01:29:22 UTC (rev 6284) +++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-08 18:51:43 UTC (rev 6285) @@ -88,6 +88,10 @@ // 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; Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-08 01:29:22 UTC (rev 6284) +++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-08 18:51:43 UTC (rev 6285) @@ -11,6 +11,7 @@ #include <stdlib.h> #include <stdio.h> #include <limits.h> +#include <float.h> #include "plan.h" @@ -65,20 +66,53 @@ 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 - plan_set_bounds(plan, - lx - plan_halfwidth, ly - plan_halfwidth, - lx + plan_halfwidth, ly + plan_halfwidth); + 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); + } - return(_plan_update_plan(plan, lx, ly, gx, gy)); + 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); } @@ -91,34 +125,28 @@ 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++,cell++) - { - cell->plan_cost = PLAN_MAX_COST; - cell->plan_next = NULL; - cell->mark = 0; - } - } - // Reset the queue heap_reset(plan->heap); // Initialize the goal cell gi = PLAN_GXWX(plan, gx); gj = PLAN_GYWY(plan, gy); - - if (!PLAN_VALID_BOUNDS(plan, gi, gj)) - return(-1); - + // 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); + + printf("bounds: (%d,%d) -> (%d,%d)\n", + plan->min_x, plan->min_y, + plan->max_x, plan->max_y); - if (!PLAN_VALID_BOUNDS(plan, li, lj)) + if(!PLAN_VALID_BOUNDS(plan, gi, gj)) return(-1); + + if(!PLAN_VALID_BOUNDS(plan, li, lj)) + return(-1); cell = plan->cells + PLAN_INDEX(plan, gi, gj); cell->plan_cost = 0; @@ -185,6 +213,68 @@ _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; + double px, py; + plan_cell_t* cell; + + // Must already have computed a global goal + if(plan->path_count == 0) + 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]; + px = PLAN_WXGX(plan, cell->ci); + py = PLAN_WXGX(plan, cell->cj); + + if((px <= plan->min_x) || (px >= plan->max_x) || + (py <= plan->min_y) || (py >= 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 + 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); } Modified: code/player/trunk/server/drivers/planner/wavefront/test.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-08 01:29:22 UTC (rev 6284) +++ code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-08 18:51:43 UTC (rev 6285) @@ -37,6 +37,7 @@ 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; @@ -86,14 +87,25 @@ 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(); - for(i=0;i<10;i++) + printf("gplan : %.6lf\n", t_p1-t_p0); + + for(i=0;i<1;i++) { // compute costs to the new goal t_p0 = get_time(); - plan_do_global(plan, lx, ly, gx, gy); + 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); @@ -101,14 +113,13 @@ draw_path(plan,lx,ly,"plan.png"); - printf("plan : %.6lf\n", t_p1-t_p0); printf("waypnt: %.6lf\n", t_w1-t_w0); puts(""); } if(plan->waypoint_count == 0) { - puts("no path"); + puts("no waypoints"); } else { This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-04-08 13:10:40
|
Revision: 6286 http://playerstage.svn.sourceforge.net/playerstage/?rev=6286&view=rev Author: gerkey Date: 2008-04-08 13:10:41 -0700 (Tue, 08 Apr 2008) Log Message: ----------- more fixes Modified Paths: -------------- code/player/trunk/server/drivers/planner/wavefront/plan.c 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-08 18:51:43 UTC (rev 6285) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-08 20:10:41 UTC (rev 6286) @@ -224,9 +224,9 @@ int i, j; plan_cell_t *cell; - cell = plan->cells; 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; @@ -256,6 +256,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 Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-08 18:51:43 UTC (rev 6285) +++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-08 20:10:41 UTC (rev 6286) @@ -124,6 +124,7 @@ int gi, gj, li,lj; float cost; plan_cell_t *cell, *ncell; + int reached_start=0; // Reset the queue heap_reset(plan->heap); @@ -138,15 +139,17 @@ printf("planning from %d,%d to %d,%d\n", li,lj,gi,gj); - printf("bounds: (%d,%d) -> (%d,%d)\n", - plan->min_x, plan->min_y, - plan->max_x, plan->max_y); - 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; @@ -198,7 +201,7 @@ // Are we done? if((ncell->ci == li) && (ncell->cj == lj)) - return(0); + reached_start=1; plan_push(plan, ncell); } @@ -206,7 +209,13 @@ } } - return(-1); + if(!reached_start) + { + puts("never found start"); + return(-1); + } + else + return(0); } int @@ -218,12 +227,14 @@ double squared_d; double squared_d_min; int li,lj; - double px, py; 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); @@ -249,17 +260,18 @@ for(c=c_min; c<plan->path_count; c++) { cell = plan->path[c]; - px = PLAN_WXGX(plan, cell->ci); - py = PLAN_WXGX(plan, cell->cj); + + //printf("step %d: (%d,%d)\n", c, cell->ci, cell->cj); - if((px <= plan->min_x) || (px >= plan->max_x) || - (py <= plan->min_y) || (py >= plan->max_y)) + 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 @@ -271,7 +283,7 @@ cell = plan->path[c-1]; - printf("ci: %d cj: %d\n", cell->ci, cell->cj); + //printf("ci: %d cj: %d\n", cell->ci, cell->cj); *gx = PLAN_WXGX(plan, cell->ci); *gy = PLAN_WYGY(plan, cell->cj); Modified: code/player/trunk/server/drivers/planner/wavefront/test.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-08 18:51:43 UTC (rev 6285) +++ code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-08 20:10:41 UTC (rev 6286) @@ -94,7 +94,16 @@ 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++) { Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-08 18:51:43 UTC (rev 6285) +++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-08 20:10:41 UTC (rev 6286) @@ -767,11 +767,17 @@ } #endif - // compute costs to the new goal - plan_do_global(this->plan, this->localize_x, this->localize_x, - 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 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-04-08 18:37:31
|
Revision: 6290 http://playerstage.svn.sourceforge.net/playerstage/?rev=6290&view=rev Author: gerkey Date: 2008-04-08 18:37:38 -0700 (Tue, 08 Apr 2008) Log Message: ----------- adding velocity control 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/wavefront.cc Modified: code/player/trunk/server/drivers/planner/wavefront/Makefile.am =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/Makefile.am 2008-04-08 21:38:51 UTC (rev 6289) +++ code/player/trunk/server/drivers/planner/wavefront/Makefile.am 2008-04-09 01:37:38 UTC (rev 6290) @@ -6,4 +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 + plan_control.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-08 21:38:51 UTC (rev 6289) +++ code/player/trunk/server/drivers/planner/wavefront/Makefile.test 2008-04-09 01:37:38 UTC (rev 6290) @@ -2,7 +2,7 @@ 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 +SRCS = test.c plan.c plan_plan.c plan_waypoint.c heap.c plan_control.c test: $(SRCS) plan.h heap.h gcc $(CFLAGS) -o $@ $(SRCS) $(LIBS) Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-08 21:38:51 UTC (rev 6289) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-09 01:37:38 UTC (rev 6290) @@ -182,18 +182,11 @@ } // Initialize the plan -void plan_init(plan_t *plan, - double res, double sx, double sy, double ox, double oy) +void plan_init(plan_t *plan) { 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++) { @@ -226,9 +219,9 @@ 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++) + for (i = plan->min_x; i <= plan->max_x; i++) { + cell = plan->cells + PLAN_INDEX(plan,i,j); cell->plan_cost = PLAN_MAX_COST; cell->plan_next = NULL; cell->mark = 0; Modified: code/player/trunk/server/drivers/planner/wavefront/plan.h =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-08 21:38:51 UTC (rev 6289) +++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-09 01:37:38 UTC (rev 6290) @@ -109,8 +109,7 @@ 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); +void plan_init(plan_t *plan); // Reset the plan void plan_reset(plan_t *plan); @@ -145,6 +144,10 @@ void plan_convert_waypoint(plan_t* plan, plan_cell_t *waypoint, double *px, double *py); +double plan_get_carrot(plan_t* plan, double* px, double* py, + double lx, double ly, + double maxdist, double distweight); + #if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO // Write the cspace occupancy distance values to a file, one per line. // Read them back in with plan_read_cspace(). Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-08 21:38:51 UTC (rev 6289) +++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-09 01:37:38 UTC (rev 6290) @@ -13,6 +13,9 @@ #include <limits.h> #include <float.h> +#include <sys/time.h> +static double get_time(void); + #include "plan.h" // Plan queue stuff @@ -27,7 +30,10 @@ { 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); @@ -35,7 +41,7 @@ plan_reset(plan); plan->path_count = 0; - if(_plan_update_plan(plan, lx, ly, gx, gy) != 0) + if(_plan_update_plan(plan, lx, ly, gx, gy) < 0) { // no path return(-1); @@ -59,6 +65,10 @@ plan->path[plan->path_count++] = cell; } + t1 = get_time(); + + printf("computed global path: %.6lf\n", t1-t0); + return(0); } @@ -69,7 +79,10 @@ 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); @@ -87,8 +100,9 @@ return(-1); } - printf("local goal: %.3lf, %.3lf\n", gx, gy); + //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"); @@ -112,6 +126,10 @@ } plan->lpath[plan->lpath_count++] = cell; } + + t1 = get_time(); + + printf("computed local path: %.6lf\n", t1-t0); return(0); } @@ -124,7 +142,6 @@ int gi, gj, li,lj; float cost; plan_cell_t *cell, *ncell; - int reached_start=0; // Reset the queue heap_reset(plan->heap); @@ -137,7 +154,7 @@ li = PLAN_GXWX(plan, lx); lj = PLAN_GYWY(plan, ly); - printf("planning from %d,%d to %d,%d\n", li,lj,gi,gj); + //printf("planning from %d,%d to %d,%d\n", li,lj,gi,gj); if(!PLAN_VALID_BOUNDS(plan, gi, gj)) { @@ -199,24 +216,19 @@ 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); } } } } - /* - if(!reached_start) + cell = plan->cells + PLAN_INDEX(plan, li, lj); + if(!cell->plan_next) { puts("never found start"); return(-1); } else - */ return(0); } @@ -314,3 +326,11 @@ else return(heap_extract_max(plan->heap)); } + +double +static get_time(void) +{ + struct timeval curr; + gettimeofday(&curr,NULL); + return(curr.tv_sec + curr.tv_usec / 1e6); +} Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-08 21:38:51 UTC (rev 6289) +++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-09 01:37:38 UTC (rev 6290) @@ -297,6 +297,8 @@ bool always_insert_rotational_waypoints; // Should map be updated on every new goal? int force_map_refresh; + // Should we do velocity control, or position control? + bool velocity_control; // methods for internal use int SetupLocalize(); @@ -388,6 +390,7 @@ this->always_insert_rotational_waypoints = cf->ReadInt(section, "add_rotational_waypoints", 1); this->force_map_refresh = cf->ReadInt(section, "force_map_refresh", 0); + this->velocity_control = cf->ReadInt(section, "velocity_control", 0); } @@ -412,8 +415,9 @@ this->new_goal = false; this->waypoint_count = 0; - this->waypoints = NULL; - this->waypoints_allocated = 0; + this->waypoints_allocated = 8; + this->waypoints = (double (*)[2])malloc(this->waypoints_allocated * + sizeof(this->waypoints[0])); if(SetupPosition() < 0) return(-1); @@ -507,14 +511,10 @@ // 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; this->plan->origin_y = info->origin.py; - // Set the bounds to search the whole grid. - plan_set_bounds(this->plan, - 0, 0, this->plan->size_x - 1, this->plan->size_y - 1); // Now get the map data, possibly in separate tiles. if(this->GetMap(true) < 0) @@ -608,6 +608,7 @@ (void*)&vel_cmd,sizeof(vel_cmd),NULL); } + this->stopped = false; } void @@ -659,8 +660,6 @@ this->waypoint_odom_x = wx_odom; this->waypoint_odom_y = wy_odom; this->waypoint_odom_a = wa_odom; - - this->stopped = false; } @@ -676,6 +675,7 @@ double replan_timediff, replan_dist; bool rotate_waypoint=false; bool replan; + int rotate_dir=0; pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL); @@ -717,7 +717,7 @@ !this->atgoal; // Did we get a new goal, or is it time to replan? - if(this->new_goal || replan) + if(this->new_goal || replan || (this->velocity_control && !this->atgoal)) { #if 0 // Should we get a new map? @@ -768,101 +768,78 @@ #endif // 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) + if(new_goal || + (this->plan->path_count == 0) || + (plan_do_local(this->plan, this->localize_x, + this->localize_y, 5.0) < 0)) { - puts("Wavefront: local plan failed"); + if(!new_goal && (this->plan->path_count != 0)) + puts("Wavefront: local plan failed"); - if(plan_do_global(this->plan, this->localize_x, this->localize_x, + // Create a global plan + if(plan_do_global(this->plan, this->localize_x, this->localize_y, this->target_x, this->target_y) < 0) puts("Wavefront: global plan failed"); + else + this->new_goal = false; } - - // 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 - if(this->plan->waypoint_count == 0) + if(!this->velocity_control) { - // No path. If we only searched a bounding box last time, grow the - // bounds to encompass the whole map and try again. - if((plan->min_x > 0) || (plan->max_x < (plan->size_x - 1)) || - (plan->min_y > 0) || (plan->max_y < (plan->size_y - 1))) - { - // Unfortunately, this computation can take a while (e.g., 1-2 - // seconds). So we'll stop the robot while it thinks. - this->StopPosition(); + // extract waypoints along the path to the goal from the current position + plan_update_waypoints(this->plan, this->localize_x, this->localize_y); - // Set the bounds to search the whole grid. - plan_set_bounds(this->plan, 0, 0, - this->plan->size_x - 1, - this->plan->size_y - 1); - - struct timeval t0, t1; - gettimeofday(&t0, NULL); - plan_update_cspace(this->plan,this->cspace_fname); - gettimeofday(&t1, NULL); - printf("time to update: %f\n", - (t1.tv_sec + t1.tv_usec/1e6) - - (t0.tv_sec + t0.tv_usec/1e6)); - - // compute costs to the new goal - plan_update_plan(this->plan, 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); - } - } -#endif - - if(this->plan->waypoint_count == 0) - { - fprintf(stderr, "Wavefront (port %d):\n " - "No path from (%.3lf,%.3lf,%.3lf) to (%.3lf,%.3lf,%.3lf)\n", - this->device_addr.robot, - this->localize_x, - this->localize_y, - RTOD(this->localize_a), - this->target_x, - this->target_y, - RTOD(this->target_a)); - // Only fail here if this is our first try at making a plan; - // if we're replanning and don't find a path then we'll just stick - // with the old plan. - if(this->curr_waypoint < 0) + if(this->plan->waypoint_count == 0) { - //this->curr_waypoint = -1; - this->new_goal=false; - this->waypoint_count = 0; + fprintf(stderr, "Wavefront (port %d):\n " + "No path from (%.3lf,%.3lf,%.3lf) to (%.3lf,%.3lf,%.3lf)\n", + this->device_addr.robot, + this->localize_x, + this->localize_y, + RTOD(this->localize_a), + this->target_x, + this->target_y, + RTOD(this->target_a)); + // Only fail here if this is our first try at making a plan; + // if we're replanning and don't find a path then we'll just stick + // with the old plan. + if(this->curr_waypoint < 0) + { + //this->curr_waypoint = -1; + this->new_goal=false; + this->waypoint_count = 0; + } } - } - else - if (this->plan->waypoint_count > this->waypoints_allocated) + else { - this->waypoints = (double (*)[2])realloc(this->waypoints, sizeof(this->waypoints[0])*this->plan->waypoint_count); - this->waypoints_allocated = this->plan->waypoint_count; + if (this->plan->waypoint_count > this->waypoints_allocated) + { + this->waypoints = (double (*)[2])realloc(this->waypoints, sizeof(this->waypoints[0])*this->plan->waypoint_count); + this->waypoints_allocated = this->plan->waypoint_count; + } + this->waypoint_count = this->plan->waypoint_count; } - this->waypoint_count = this->plan->waypoint_count; - if(this->plan->waypoint_count > 0) - { - for(int i=0;i<this->plan->waypoint_count;i++) + if(this->plan->waypoint_count > 0) { - double wx, wy; - plan_convert_waypoint(this->plan, - this->plan->waypoints[i], - &wx, &wy); - this->waypoints[i][0] = wx; - this->waypoints[i][1] = wy; + for(int i=0;i<this->plan->waypoint_count;i++) + { + double wx, wy; + plan_convert_waypoint(this->plan, + this->plan->waypoints[i], + &wx, &wy); + this->waypoints[i][0] = wx; + this->waypoints[i][1] = wy; + } + + this->curr_waypoint = 0; + // Why is this here? + this->new_goal = true; } - - this->curr_waypoint = 0; - this->new_goal = true; + last_replan_time = t; + last_replan_lx = this->localize_x; + last_replan_ly = this->localize_y; } - last_replan_time = t; - last_replan_lx = this->localize_x; - last_replan_ly = this->localize_y; } if(!this->enable) @@ -872,95 +849,190 @@ continue; } - bool going_for_target = (this->curr_waypoint == this->plan->waypoint_count); - dist = sqrt(((this->localize_x - this->target_x) * - (this->localize_x - this->target_x)) + - ((this->localize_y - this->target_y) * - (this->localize_y - this->target_y))); - // Note that we compare the current heading and waypoint heading in the - // *odometric* frame. We do this because comparing the current - // heading and waypoint heading in the localization frame is unreliable - // when making small adjustments to achieve a desired heading (i.e., the - // robot gets there and VFH stops, but here we don't realize we're done - // because the localization heading hasn't changed sufficiently). - angle = fabs(this->angle_diff(this->waypoint_odom_a,this->position_a)); - if(going_for_target && dist < this->dist_eps && angle < this->ang_eps) + + if(this->velocity_control) { - // we're at the final target, so stop - StopPosition(); - this->curr_waypoint = -1; - this->new_goal = false; - this->atgoal = true; + if(this->plan->path_count && !this->atgoal) + { + // Check doneness + dist = sqrt(((this->localize_x - this->target_x) * + (this->localize_x - this->target_x)) + + ((this->localize_y - this->target_y) * + (this->localize_y - this->target_y))); + angle = fabs(this->angle_diff(this->target_a,this->localize_a)); + if((dist < this->dist_eps) && (angle < this->ang_eps)) + { + this->StopPosition(); + this->new_goal = false; + this->curr_waypoint = -1; + this->atgoal = true; + } + else + { + // Compute velocities + double wx, wy; + double maxd=2.0; + double distweight=10.0; + + if(plan_get_carrot(this->plan, &wx, &wy, + this->localize_x, this->localize_y, + maxd, distweight) < 0) + { + puts("Failed to find a carrot"); + this->StopPosition(); + } + else + { + // Establish fake waypoint, for client-side visualization + this->curr_waypoint = 0; + this->waypoint_count = 2; + this->waypoints[0][0] = this->localize_x; + this->waypoints[0][1] = this->localize_y; + this->waypoints[1][0] = wx; + this->waypoints[1][1] = wy; + + // TODO: expose these control params in the .cfg file + double tvmin = 0.1; + double tvmax = 0.5; + double avmin = DTOR(10.0); + double avmax = DTOR(45.0); + double amin = DTOR(5.0); + double amax = DTOR(20.0); + + double d = sqrt((this->localize_x-wx)*(this->localize_x-wx) + + (this->localize_y-wy)*(this->localize_y-wy)); + double b = atan2(wy - this->localize_y, wx - this->localize_x); + + double av,tv; + double a = amin + (d / maxd) * (amax-amin); + double ad = angle_diff(b, this->localize_a); + + // Are we on top of the goal? + if(d < this->dist_eps) + { + if(!rotate_dir) + { + if(ad < 0) + rotate_dir = -1; + else + rotate_dir = 1; + } + + tv = 0.0; + av = rotate_dir * (avmin + (fabs(ad)/M_PI) * (avmax-avmin)); + } + else + { + rotate_dir = 0; + + if(fabs(ad) > a) + tv = 0.0; + else + tv = tvmin + (d / (M_SQRT2 * maxd)) * (tvmax-tvmin); + + av = avmin + (fabs(ad)/M_PI) * (avmax-avmin); + if(ad < 0) + av = -av; + } + + this->PutPositionCommand(tv,0.0,av,0); + } + } + } + else + this->StopPosition(); } - else if(this->curr_waypoint < 0) + else // !velocity_control { - // no more waypoints, so stop - StopPosition(); - } - else - { - // are we there yet? ignore angle, cause this is just a waypoint - dist = sqrt(((this->localize_x - this->waypoint_x) * - (this->localize_x - this->waypoint_x)) + - ((this->localize_y - this->waypoint_y) * - (this->localize_y - this->waypoint_y))); + bool going_for_target = (this->curr_waypoint == this->plan->waypoint_count); + dist = sqrt(((this->localize_x - this->target_x) * + (this->localize_x - this->target_x)) + + ((this->localize_y - this->target_y) * + (this->localize_y - this->target_y))); // Note that we compare the current heading and waypoint heading in the // *odometric* frame. We do this because comparing the current // heading and waypoint heading in the localization frame is unreliable // when making small adjustments to achieve a desired heading (i.e., the // robot gets there and VFH stops, but here we don't realize we're done // because the localization heading hasn't changed sufficiently). - if(this->new_goal || - (rotate_waypoint && - (fabs(this->angle_diff(this->waypoint_odom_a,this->position_a)) - < M_PI/4.0)) || - (!rotate_waypoint && (dist < this->dist_eps))) + angle = fabs(this->angle_diff(this->waypoint_odom_a,this->position_a)); + if(going_for_target && dist < this->dist_eps && angle < this->ang_eps) { - if(this->curr_waypoint == this->waypoint_count) + // we're at the final target, so stop + StopPosition(); + this->curr_waypoint = -1; + this->new_goal = false; + this->atgoal = true; + } + else if(this->curr_waypoint < 0) + { + // no more waypoints, so stop + StopPosition(); + } + else + { + // are we there yet? ignore angle, cause this is just a waypoint + dist = sqrt(((this->localize_x - this->waypoint_x) * + (this->localize_x - this->waypoint_x)) + + ((this->localize_y - this->waypoint_y) * + (this->localize_y - this->waypoint_y))); + // Note that we compare the current heading and waypoint heading in the + // *odometric* frame. We do this because comparing the current + // heading and waypoint heading in the localization frame is unreliable + // when making small adjustments to achieve a desired heading (i.e., the + // robot gets there and VFH stops, but here we don't realize we're done + // because the localization heading hasn't changed sufficiently). + if(this->new_goal || + (rotate_waypoint && + (fabs(this->angle_diff(this->waypoint_odom_a,this->position_a)) + < M_PI/4.0)) || + (!rotate_waypoint && (dist < this->dist_eps))) { - // no more waypoints, so wait for target achievement + if(this->curr_waypoint == this->waypoint_count) + { + // no more waypoints, so wait for target achievement - //puts("waiting for goal achievement"); - usleep(CYCLE_TIME_US); - continue; - } - // get next waypoint - this->waypoint_x = this->waypoints[this->curr_waypoint][0]; - this->waypoint_y = this->waypoints[this->curr_waypoint][1]; - this->curr_waypoint++; + //puts("waiting for goal achievement"); + usleep(CYCLE_TIME_US); + continue; + } + // get next waypoint + this->waypoint_x = this->waypoints[this->curr_waypoint][0]; + this->waypoint_y = this->waypoints[this->curr_waypoint][1]; + this->curr_waypoint++; - this->waypoint_a = this->target_a; - if(this->always_insert_rotational_waypoints || - (this->curr_waypoint == 2)) - { - dist = sqrt((this->waypoint_x - this->localize_x) * - (this->waypoint_x - this->localize_x) + - (this->waypoint_y - this->localize_y) * - (this->waypoint_y - this->localize_y)); - angle = atan2(this->waypoint_y - this->localize_y, - this->waypoint_x - this->localize_x); - if((dist > this->dist_eps) && - fabs(this->angle_diff(angle,this->localize_a)) > M_PI/4.0) + this->waypoint_a = this->target_a; + if(this->always_insert_rotational_waypoints || + (this->curr_waypoint == 2)) { - this->waypoint_x = this->localize_x; - this->waypoint_y = this->localize_y; - this->waypoint_a = angle; - this->curr_waypoint--; - rotate_waypoint=true; + dist = sqrt((this->waypoint_x - this->localize_x) * + (this->waypoint_x - this->localize_x) + + (this->waypoint_y - this->localize_y) * + (this->waypoint_y - this->localize_y)); + angle = atan2(this->waypoint_y - this->localize_y, + this->waypoint_x - this->localize_x); + if((dist > this->dist_eps) && + fabs(this->angle_diff(angle,this->localize_a)) > M_PI/4.0) + { + this->waypoint_x = this->localize_x; + this->waypoint_y = this->localize_y; + this->waypoint_a = angle; + this->curr_waypoint--; + rotate_waypoint=true; + } + else + rotate_waypoint=false; } else rotate_waypoint=false; + + this->new_goal = false; } - else - rotate_waypoint=false; - this->new_goal = false; + SetWaypoint(this->waypoint_x, this->waypoint_y, this->waypoint_a); } - - SetWaypoint(this->waypoint_x, this->waypoint_y, this->waypoint_a); } - usleep(CYCLE_TIME_US); } } @@ -1118,6 +1190,10 @@ oj += sj; } } + + plan_init(this->plan); + plan_compute_cspace(this->plan); + return(0); } @@ -1132,7 +1208,6 @@ { 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; @@ -1144,14 +1219,10 @@ // 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->plan->origin_y = info->origin.py; - // Set the bounds to search the whole grid. - plan_set_bounds(this->plan, - 0, 0, this->plan->size_x - 1, this->plan->size_y - 1); delete msg; return(0); @@ -1190,9 +1261,6 @@ if(this->GetMap(false) < 0) return(-1); - //plan_update_cspace(this->plan,this->cspace_fname); - plan_compute_cspace(this->plan); - this->have_map = true; this->new_map = true; @@ -1271,9 +1339,6 @@ // 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_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. |
From: <ge...@us...> - 2008-04-14 16:15:09
|
Revision: 6321 http://playerstage.svn.sourceforge.net/playerstage/?rev=6321&view=rev Author: gerkey Date: 2008-04-14 16:15:16 -0700 (Mon, 14 Apr 2008) Log Message: ----------- more planner tweaks Modified Paths: -------------- code/player/trunk/server/drivers/planner/wavefront/plan.h code/player/trunk/server/drivers/planner/wavefront/wavefront.cc Modified: code/player/trunk/server/drivers/planner/wavefront/plan.h =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-14 21:43:50 UTC (rev 6320) +++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-14 23:15:16 UTC (rev 6321) @@ -148,6 +148,8 @@ double lx, double ly, double maxdist, double distweight); +void plan_set_obstacles(plan_t* plan, double* obs, size_t num); + #if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO // Write the cspace occupancy distance values to a file, one per line. // Read them back in with plan_read_cspace(). Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-14 21:43:50 UTC (rev 6320) +++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-14 23:15:16 UTC (rev 6321) @@ -237,6 +237,7 @@ player_devaddr_t position_id; player_devaddr_t localize_id; player_devaddr_t map_id; + player_devaddr_t laser_id; double map_res; double robot_radius; double safety_dist; @@ -253,6 +254,7 @@ Device* position; Device* localize; Device* mapdevice; + Device* laser; // are we disabled? bool enable; @@ -299,19 +301,31 @@ int force_map_refresh; // Should we do velocity control, or position control? bool velocity_control; + // How many laser scans should we buffer? + int scans_size; + // The scan buffer + player_laser_data_scanpose_t* scans; + int scans_count; + int scans_idx; + double* scan_points; + int scan_points_size; + int scan_points_count; // methods for internal use int SetupLocalize(); + int SetupLaser(); int SetupPosition(); int SetupMap(); int GetMap(bool threaded); int GetMapInfo(bool threaded); int ShutdownPosition(); int ShutdownLocalize(); + int ShutdownLaser(); int ShutdownMap(); double angle_diff(double a, double b); void ProcessCommand(player_planner_cmd_t* cmd); + void ProcessLaserScan(player_laser_data_scanpose_t* data); void ProcessLocalizeData(player_position2d_data_t* data); void ProcessPositionData(player_position2d_data_t* data); void ProcessMapInfo(player_map_info_t* info); @@ -378,6 +392,12 @@ this->SetError(-1); return; } + + // Can use a map device + memset(&this->laser_id,0,sizeof(player_devaddr_t)); + cf->ReadDeviceAddr(&this->laser_id, section, "requires", + PLAYER_LASER_CODE, -1, NULL); + this->safety_dist = cf->ReadLength(section,"safety_dist", 0.25); this->max_radius = cf->ReadLength(section,"max_radius",1.0); this->dist_penalty = cf->ReadFloat(section,"dist_penalty",1.0); @@ -391,6 +411,17 @@ cf->ReadInt(section, "add_rotational_waypoints", 1); this->force_map_refresh = cf->ReadInt(section, "force_map_refresh", 0); this->velocity_control = cf->ReadInt(section, "velocity_control", 0); + if(this->laser_id.interf) + { + this->scans_size = cf->ReadInt(section, "laser_buffer_size", 10); + if(this->scans_size < 1) + { + PLAYER_WARN("must buffer at least one laser scan"); + this->scans_size = 1; + } + } + else + this->scans_size = 0; } @@ -436,6 +467,22 @@ if(SetupLocalize() < 0) return(-1); + if(this->laser_id.interf) + { + if(SetupLaser() < 0) + return(-1); + + this->scans = + (player_laser_data_scanpose_t*)malloc(this->scans_size * + sizeof(player_laser_data_scanpose_t)); + assert(this->scans); + this->scans_idx = 0; + this->scans_count = 0; + this->scan_points = NULL; + this->scan_points_size = 0; + this->scan_points_count = 0; + } + // Start the driver thread. this->StartThread(); return 0; @@ -458,6 +505,8 @@ ShutdownPosition(); ShutdownLocalize(); ShutdownMap(); + if(this->laser_id.interf) + ShutdownLaser(); return 0; } @@ -489,6 +538,69 @@ #endif } +void +Wavefront::ProcessLaserScan(player_laser_data_scanpose_t* data) +{ + // update counters; + this->scans_idx = (this->scans_idx + 1) % this->scans_size; + this->scans_count++; + this->scans_count = MIN(this->scans_count,this->scans_size); + + // copy in the new scan + memcpy(this->scans+this->scans_idx, data, + sizeof(player_laser_data_scanpose_t)); + + // run through the scans to see how much room we need to store all the + // hitpoints + int hitpt_cnt=0; + player_laser_data_scanpose_t* scan = this->scans; + for(int i=0;i<this->scans_count;i++,scan++) + hitpt_cnt += scan->scan.ranges_count*2; + + // allocate more space as necessary + if(this->scan_points_size < hitpt_cnt) + { + this->scan_points_size = hitpt_cnt; + this->scan_points = (double*)realloc(this->scan_points, + this->scan_points_size); + assert(this->scan_points); + } + + // project hit points from each scan + scan = this->scans; + double* pts = this->scan_points; + this->scan_points_count = 0; + for(int i=0;i<this->scans_count;i++,scan++) + { + float b=scan->scan.min_angle; + float* r=scan->scan.ranges; + for(unsigned int j=0; + j<scan->scan.ranges_count; + j++,r++,b+=scan->scan.resolution) + { + double rx, ry; + rx = (*r)*cos(b); + ry = (*r)*sin(b); + + double cs,sn; + cs = cos(scan->pose.pa); + sn = sin(scan->pose.pa); + + double lx,ly; + lx = scan->pose.px + rx*cs - ry*sn; + ly = scan->pose.py + rx*sn + ry*cs; + + assert(this->scan_points_count*2 < this->scan_points_size); + *(pts++) = lx; + *(pts++) = ly; + this->scan_points_count++; + } + } + + printf("setting %d hit points\n", this->scan_points_count); + plan_set_obstacles(plan, this->scan_points, this->scan_points_count); +} + void Wavefront::ProcessLocalizeData(player_position2d_data_t* data) { @@ -1099,6 +1211,26 @@ } //////////////////////////////////////////////////////////////////////////////// +// Set up the underlying laser device. +int +Wavefront::SetupLaser() +{ + // Subscribe to the laser device. + if(!(this->laser = deviceTable->GetDevice(this->laser_id))) + { + PLAYER_ERROR("unable to locate suitable laser device"); + return(-1); + } + if(this->laser->Subscribe(this->InQueue) != 0) + { + PLAYER_ERROR("unable to subscribe to laser device"); + return(-1); + } + + return(0); +} + +//////////////////////////////////////////////////////////////////////////////// // Set up the underlying localize device. int Wavefront::SetupLocalize() @@ -1283,6 +1415,12 @@ } int +Wavefront::ShutdownLaser() +{ + return(this->laser->Unsubscribe(this->InQueue)); +} + +int Wavefront::ShutdownMap() { return(this->mapdevice->Unsubscribe(this->InQueue)); @@ -1412,6 +1550,15 @@ this->new_map_available = true; return(0); } + // Is it a pose-stamped laser scan? + else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, + PLAYER_LASER_DATA_SCANPOSE, + this->laser_id)) + { + player_laser_data_scanpose_t* pdata = (player_laser_data_scanpose_t*)data; + this->ProcessLaserScan(pdata); + return(0); + } else return(-1); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-04-15 17:52:48
|
Revision: 6326 http://playerstage.svn.sourceforge.net/playerstage/?rev=6326&view=rev Author: gerkey Date: 2008-04-15 17:52:54 -0700 (Tue, 15 Apr 2008) Log Message: ----------- fixed laser-detected obstacles; debugging local plan-switching 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_control.c 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-15 20:51:45 UTC (rev 6325) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-16 00:52:54 UTC (rev 6326) @@ -36,6 +36,9 @@ #define HASH_LEN (MD5_DIGEST_LENGTH / sizeof(unsigned int)) #endif +#include <sys/time.h> +static double get_time(void); + #if 0 void draw_cspace(plan_t* plan, const char* fname); #endif @@ -73,42 +76,40 @@ plan_set_obstacles(plan_t* plan, double* obs, size_t num) { size_t i; + int j; int di,dj; float* p; plan_cell_t* cell, *ncell; + double t0,t1; - // Remove any previous obstacles - for(i=0;i<plan->obs_pts_num;i++) + t0 = get_time(); + + // Start with static obstacle data + cell = plan->cells; + for(j=0;j<plan->size_y*plan->size_x;j++,cell++) { - 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; + cell->mark = 0; } - // Do we need more room? - if(num > plan->obs_pts_size) + // Expand around the dynamic obstacle pts + for(i=0;i<num;i++) { - 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; + if(!PLAN_VALID(plan,gx,gy)) + continue; + cell = plan->cells + PLAN_INDEX(plan,gx,gy); + if(cell->mark) + continue; + + cell->mark = 1; cell->occ_state_dyn = 1; cell->occ_dist_dyn = 0.0; @@ -130,6 +131,9 @@ } } } + + t1 = get_time(); + printf("plan_set_obstacles: %.6lf\n", t1-t0); } void @@ -375,10 +379,9 @@ } } -#if 0 #include <gdk-pixbuf/gdk-pixbuf.h> - void +void draw_cspace(plan_t* plan, const char* fname) { GdkPixbuf* pixbuf; @@ -396,7 +399,8 @@ for(i=0;i<plan->size_x;i++,p++) { paddr = p * 3; - if(plan->cells[PLAN_INDEX(plan,i,j)].occ_state == 1) + //if(plan->cells[PLAN_INDEX(plan,i,j)].occ_state == 1) + if(plan->cells[PLAN_INDEX(plan,i,j)].occ_dist < plan->abs_min_radius) { pixels[paddr] = 255; pixels[paddr+1] = 0; @@ -535,7 +539,6 @@ gdk_pixbuf_unref(pixbuf); free(pixels); } -#endif // Construct the configuration space from the occupancy grid. // This treats both occupied and unknown cells as bad. @@ -711,3 +714,11 @@ MD5_Final((unsigned char*)digest,&c); } #endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO + +double +static get_time(void) +{ + struct timeval curr; + gettimeofday(&curr,NULL); + return(curr.tv_sec + curr.tv_usec / 1e6); +} Modified: code/player/trunk/server/drivers/planner/wavefront/plan.h =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-15 20:51:45 UTC (rev 6325) +++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-16 00:52:54 UTC (rev 6326) @@ -72,11 +72,6 @@ // The grid data plan_cell_t *cells; - // 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; Modified: code/player/trunk/server/drivers/planner/wavefront/plan_control.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-04-15 20:51:45 UTC (rev 6325) +++ code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-04-16 00:52:54 UTC (rev 6326) @@ -1,4 +1,5 @@ #include <stdlib.h> +#include <assert.h> #include "plan.h" @@ -18,6 +19,10 @@ cell = plan->cells + PLAN_INDEX(plan,li,lj); + printf("finding carrot from (%d,%d) : %.3lf\n", + cell->ci, cell->cj, cell->occ_dist_dyn); + assert(cell->occ_dist_dyn >= plan->abs_min_radius); + // Step back from maxdist, looking for the best carrot bestcost = -1.0; for(dist = maxdist; dist >= plan->scale; dist -= plan->scale) @@ -30,7 +35,11 @@ // Check whether the straight-line path is clear if((cost = _plan_check_path(plan, cell, ncell)) < 0.0) + { + printf("no path from (%d,%d) to (%d,%d)\n", + cell->ci, cell->cj, ncell->ci, ncell->cj); continue; + } // Weight distance cost += distweight * (1.0/(dist*dist)); Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-15 20:51:45 UTC (rev 6325) +++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-16 00:52:54 UTC (rev 6326) @@ -85,9 +85,9 @@ // Set bounds as directed xmin = PLAN_GXWX(plan, lx - plan_halfwidth); - ymin = PLAN_GXWX(plan, ly - plan_halfwidth); + ymin = PLAN_GYWY(plan, ly - plan_halfwidth); xmax = PLAN_GXWX(plan, lx + plan_halfwidth); - ymax = PLAN_GXWX(plan, ly + plan_halfwidth); + ymax = PLAN_GYWY(plan, ly + plan_halfwidth); plan_set_bounds(plan, xmin, ymin, xmax, ymax); // Reset plan costs (within the local patch) @@ -142,6 +142,8 @@ 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); @@ -153,7 +155,7 @@ // 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)) @@ -168,6 +170,13 @@ 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 = plan->max_radius; + cell = plan->cells + PLAN_INDEX(plan, gi, gj); cell->plan_cost = 0; plan_push(plan, cell); @@ -222,7 +231,11 @@ } } + // 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"); @@ -253,6 +266,8 @@ 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; Modified: code/player/trunk/server/drivers/planner/wavefront/test.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-15 20:51:45 UTC (rev 6325) +++ code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-16 00:52:54 UTC (rev 6326) @@ -33,9 +33,9 @@ int i, j; plan_t* plan; - double robot_radius=0.25; - double safety_dist=0.2; - double max_radius=1.0; + double robot_radius=0.16; + double safety_dist=0.05; + double max_radius=0.25; double dist_penalty=1.0;; double plan_halfwidth = 5.0; @@ -76,9 +76,15 @@ } } free(mapdata); + + plan->scale = res; + plan->size_x = sx; + plan->size_y = sy; + plan->origin_x = 0.0; + plan->origin_y = 0.0; // Do initialization - plan_init(plan, res, sx, sy, 0.0, 0.0); + plan_init(plan); t_c0 = get_time(); plan_compute_cspace(plan); Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-15 20:51:45 UTC (rev 6325) +++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-16 00:52:54 UTC (rev 6326) @@ -219,9 +219,14 @@ #include <assert.h> #include <math.h> + #include <libplayercore/playercore.h> #include "plan.h" +#include <sys/time.h> +static double get_time(void); +extern "C" { void draw_cspace(plan_t* plan, const char* fname); } + // TODO: monitor localize timestamps, and slow or stop robot accordingly // time to sleep between loops (us) @@ -305,6 +310,8 @@ bool velocity_control; // How many laser scans should we buffer? int scans_size; + // How far out do we insert obstacles? + double scan_maxrange; // The scan buffer player_laser_data_scanpose_t* scans; int scans_count; @@ -428,6 +435,7 @@ PLAYER_WARN("must buffer at least one laser scan"); this->scans_size = 1; } + this->scan_maxrange = cf->ReadLength(section, "laser_maxrange", 6.0); } else this->scans_size = 0; @@ -558,14 +566,28 @@ void Wavefront::ProcessLaserScan(player_laser_data_scanpose_t* data) { - // update counters; - this->scans_idx = (this->scans_idx + 1) % this->scans_size; + double t0,t1; + + t0 = get_time(); + + // free up the old scan, if we're replacing one + if(this->scans_idx < this->scans_count) + playerxdr_cleanup_message(this->scans+this->scans_idx, + PLAYER_LASER_CODE, PLAYER_MSGTYPE_DATA, + PLAYER_LASER_DATA_SCANPOSE); + // copy in the new scan + playerxdr_deepcopy_message(data, this->scans+this->scans_idx, + PLAYER_LASER_CODE, PLAYER_MSGTYPE_DATA, + PLAYER_LASER_DATA_SCANPOSE); + //memcpy(this->scans+this->scans_idx, data, + //sizeof(player_laser_data_scanpose_t)); + + // update counters this->scans_count++; this->scans_count = MIN(this->scans_count,this->scans_size); + this->scans_idx = (this->scans_idx + 1) % this->scans_size; - // copy in the new scan - memcpy(this->scans+this->scans_idx, data, - sizeof(player_laser_data_scanpose_t)); + printf("%d scans\n", this->scans_count); // run through the scans to see how much room we need to store all the // hitpoints @@ -579,7 +601,7 @@ { this->scan_points_size = hitpt_cnt; this->scan_points = (double*)realloc(this->scan_points, - this->scan_points_size); + this->scan_points_size*sizeof(double)); assert(this->scan_points); } @@ -595,17 +617,19 @@ j<scan->scan.ranges_count; j++,r++,b+=scan->scan.resolution) { - double rx, ry; - rx = (*r)*cos(b); - ry = (*r)*sin(b); + if(((*r) >= this->scan_maxrange) || ((*r) >= scan->scan.max_range)) + continue; + //double rx, ry; + //rx = (*r)*cos(b); + //ry = (*r)*sin(b); double cs,sn; - cs = cos(scan->pose.pa); - sn = sin(scan->pose.pa); + cs = cos(scan->pose.pa+b); + sn = sin(scan->pose.pa+b); double lx,ly; - lx = scan->pose.px + rx*cs - ry*sn; - ly = scan->pose.py + rx*sn + ry*cs; + lx = scan->pose.px + (*r)*cs; + ly = scan->pose.py + (*r)*sn; assert(this->scan_points_count*2 < this->scan_points_size); *(pts++) = lx; @@ -617,6 +641,10 @@ printf("setting %d hit points\n", this->scan_points_count); plan_set_obstacles(plan, this->scan_points, this->scan_points_count); + t1 = get_time(); + printf("ProcessLaserScan: %.6lf\n", t1-t0); + +#if 0 if(this->graphics2d_id.interf) { // Clear the canvas @@ -631,9 +659,9 @@ hitpt_cnt/2)); pts.points_count = hitpt_cnt/2; pts.color.alpha = 0; - pts.color.red = 0; + pts.color.red = 255; pts.color.blue = 0; - pts.color.green = 255; + pts.color.green = 0; for(int i=0;i<hitpt_cnt/2;i++) { pts.points[i].px = this->scan_points[2*i]; @@ -646,6 +674,7 @@ (void*)&pts,0,NULL); free(pts.points); } +#endif } void @@ -863,6 +892,13 @@ this->PutPlannerData(); } + if(!this->enable) + { + this->StopPosition(); + usleep(CYCLE_TIME_US); + continue; + } + // Is it time to replan? replan_timediff = t - last_replan_time; replan_dist = sqrt(((this->localize_x - last_replan_lx) * @@ -925,12 +961,15 @@ this->new_map = false; } #endif + double t0,t1; + t0 = get_time(); + // compute costs to the new goal. Try local plan first if(new_goal || (this->plan->path_count == 0) || (plan_do_local(this->plan, this->localize_x, - this->localize_y, 5.0) < 0)) + this->localize_y, this->scan_maxrange) < 0)) { if(!new_goal && (this->plan->path_count != 0)) puts("Wavefront: local plan failed"); @@ -943,6 +982,35 @@ this->new_goal = false; } + if(this->graphics2d_id.interf && this->plan->lpath_count) + { + this->graphics2d->PutMsg(this->InQueue, + PLAYER_MSGTYPE_CMD, + PLAYER_GRAPHICS2D_CMD_CLEAR, + NULL,0,NULL); + + player_graphics2d_cmd_polyline_t line; + line.points_count = this->plan->lpath_count; + line.points = (player_point_2d_t*)malloc(sizeof(player_point_2d_t)*line.points_count); + line.color.alpha = 0; + line.color.red = 0; + line.color.green = 255; + line.color.blue = 0; + for(int i=0;i<this->plan->lpath_count;i++) + { + line.points[i].px = PLAN_WXGX(this->plan,this->plan->lpath[i]->ci); + line.points[i].py = PLAN_WYGY(this->plan,this->plan->lpath[i]->cj); + } + this->graphics2d->PutMsg(this->InQueue, + PLAYER_MSGTYPE_CMD, + PLAYER_GRAPHICS2D_CMD_POLYLINE, + (void*)&line,0,NULL); + free(line.points); + } + + t1 = get_time(); + printf("planning: %.6lf\n", t1-t0); + if(!this->velocity_control) { // extract waypoints along the path to the goal from the current position @@ -1001,16 +1069,11 @@ } } - if(!this->enable) - { - this->StopPosition(); - usleep(CYCLE_TIME_US); - continue; - } - if(this->velocity_control) { + double t0, t1; + t0 = get_time(); if(this->plan->path_count && !this->atgoal) { // Check doneness @@ -1033,12 +1096,16 @@ double maxd=2.0; double distweight=10.0; + printf("pose: (%.3lf,%.3lf,%.3lf)\n", + this->localize_x, this->localize_y, RTOD(this->localize_a)); if(plan_get_carrot(this->plan, &wx, &wy, this->localize_x, this->localize_y, maxd, distweight) < 0) { puts("Failed to find a carrot"); + draw_cspace(this->plan, "debug.png"); this->StopPosition(); + exit(-1); } else { @@ -1101,6 +1168,9 @@ } else this->StopPosition(); + + t1 = get_time(); + printf("control: %.6lf\n", t1-t0); } else // !velocity_control { @@ -1249,9 +1319,12 @@ // take the bigger of the two dimensions, convert to meters, and halve // to get a radius - this->robot_radius = MAX(geom->size.sl, geom->size.sw); + //this->robot_radius = MAX(geom->size.sl, geom->size.sw); + this->robot_radius = geom->size.sw; this->robot_radius /= 2.0; + printf("robot radius: %.3lf\n", this->robot_radius); + delete msg; return 0; @@ -1391,6 +1464,7 @@ plan_init(this->plan); plan_compute_cspace(this->plan); + draw_cspace(this->plan,"cspace.png"); return(0); } @@ -1650,3 +1724,11 @@ else return(d2); } + +double +static get_time(void) +{ + struct timeval curr; + gettimeofday(&curr,NULL); + return(curr.tv_sec + curr.tv_usec / 1e6); +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-04-15 19:13:04
|
Revision: 6327 http://playerstage.svn.sourceforge.net/playerstage/?rev=6327&view=rev Author: gerkey Date: 2008-04-15 19:13:10 -0700 (Tue, 15 Apr 2008) Log Message: ----------- added path hysteresis Modified Paths: -------------- code/player/trunk/server/drivers/planner/wavefront/Makefile.am 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_control.c code/player/trunk/server/drivers/planner/wavefront/plan_plan.c code/player/trunk/server/drivers/planner/wavefront/wavefront.cc Modified: code/player/trunk/server/drivers/planner/wavefront/Makefile.am =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/Makefile.am 2008-04-16 00:52:54 UTC (rev 6326) +++ code/player/trunk/server/drivers/planner/wavefront/Makefile.am 2008-04-16 02:13:10 UTC (rev 6327) @@ -3,7 +3,7 @@ noinst_LTLIBRARIES += libwavefront.la endif -AM_CPPFLAGS = -Wall -g -I$(top_srcdir) `pkg-config --cflags gdk-pixbuf-2.0` +AM_CPPFLAGS = -Wall -g -O3 -I$(top_srcdir) `pkg-config --cflags gdk-pixbuf-2.0` libwavefront_la_SOURCES = plan.c plan_plan.c plan_waypoint.c plan.h \ plan_control.c wavefront.cc heap.h heap.c Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-16 00:52:54 UTC (rev 6326) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-16 02:13:10 UTC (rev 6327) @@ -45,7 +45,8 @@ // Create a planner plan_t *plan_alloc(double abs_min_radius, double des_min_radius, - double max_radius, double dist_penalty) + double max_radius, double dist_penalty, + double hysteresis_factor) { plan_t *plan; @@ -56,6 +57,7 @@ plan->max_radius = max_radius; plan->dist_penalty = dist_penalty; + plan->hysteresis_factor = hysteresis_factor; plan->heap = heap_alloc(PLAN_DEFAULT_HEAP_SIZE, (heap_free_elt_fn_t)NULL); assert(plan->heap); Modified: code/player/trunk/server/drivers/planner/wavefront/plan.h =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-16 00:52:54 UTC (rev 6326) +++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-04-16 02:13:10 UTC (rev 6327) @@ -37,6 +37,8 @@ // Mark used in dynamic programming char mark; + // Mark used in path hysterisis + char lpathmark; // The next cell in the plan struct _plan_cell_t *plan_next; @@ -68,6 +70,9 @@ // Penalty factor for cells inside the max radius double dist_penalty; + + // Cost multiplier for cells on the previous local path + double hysteresis_factor; // The grid data plan_cell_t *cells; @@ -95,8 +100,11 @@ // Create a planner -plan_t *plan_alloc(double abs_min_radius, double des_min_radius, - double max_radius, double dist_penalty); +plan_t *plan_alloc(double abs_min_radius, + double des_min_radius, + double max_radius, + double dist_penalty, + double hysteresis_factor); void plan_compute_dist_kernel(plan_t* plan); Modified: code/player/trunk/server/drivers/planner/wavefront/plan_control.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-04-16 00:52:54 UTC (rev 6326) +++ code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-04-16 02:13:10 UTC (rev 6327) @@ -13,15 +13,20 @@ int li, lj; double dist, d; double cost, bestcost; + char old_occ_state; + float old_occ_dist; li = PLAN_GXWX(plan, lx); lj = PLAN_GYWY(plan, ly); cell = plan->cells + PLAN_INDEX(plan,li,lj); - printf("finding carrot from (%d,%d) : %.3lf\n", - cell->ci, cell->cj, cell->occ_dist_dyn); - assert(cell->occ_dist_dyn >= plan->abs_min_radius); + // 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 = plan->max_radius; // Step back from maxdist, looking for the best carrot bestcost = -1.0; @@ -50,6 +55,11 @@ *py = PLAN_WYGY(plan,ncell->cj); } } + + // 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; return(bestcost); } Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-16 00:52:54 UTC (rev 6326) +++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-16 02:13:10 UTC (rev 6327) @@ -80,6 +80,7 @@ int xmin,ymin,xmax,ymax; plan_cell_t* cell; double t0,t1; + int i; t0 = get_time(); @@ -112,6 +113,11 @@ li = PLAN_GXWX(plan, lx); lj = PLAN_GYWY(plan, ly); + // Reset path marks (TODO: find a smarter place to do this) + cell = plan->cells; + for(i=0;i<plan->size_x*plan->size_y;i++,cell++) + cell->lpathmark = 0; + // Cache the path for(cell = plan->cells + PLAN_INDEX(plan,li,lj); cell; @@ -125,6 +131,7 @@ assert(plan->lpath); } plan->lpath[plan->lpath_count++] = cell; + cell->lpathmark = 1; } t1 = get_time(); @@ -215,7 +222,11 @@ if (ncell->occ_dist_dyn < plan->abs_min_radius) continue; - cost = cell->plan_cost + *p; + cost = cell->plan_cost; + if(ncell->lpathmark) + cost += (*p) * plan->hysteresis_factor; + else + cost += *p; if(ncell->occ_dist_dyn < plan->max_radius) cost += plan->dist_penalty * (plan->max_radius - ncell->occ_dist_dyn); Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-16 00:52:54 UTC (rev 6326) +++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-16 02:13:10 UTC (rev 6327) @@ -473,7 +473,8 @@ if(!(this->plan = plan_alloc(this->robot_radius+this->safety_dist, this->robot_radius+this->safety_dist, this->max_radius, - this->dist_penalty))) + this->dist_penalty, + 0.5))) { PLAYER_ERROR("failed to allocate plan"); return(-1); @@ -1613,7 +1614,8 @@ this->plan = plan_alloc(this->robot_radius+this->safety_dist, this->robot_radius+this->safety_dist, this->max_radius, - this->dist_penalty); + this->dist_penalty, + 0.5); assert(this->plan); // Fill in the map structure This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-04-16 10:23:22
|
Revision: 6336 http://playerstage.svn.sourceforge.net/playerstage/?rev=6336&view=rev Author: gerkey Date: 2008-04-16 10:23:26 -0700 (Wed, 16 Apr 2008) Log Message: ----------- debugging controller Modified Paths: -------------- code/player/trunk/server/drivers/planner/wavefront/plan_control.c code/player/trunk/server/drivers/planner/wavefront/wavefront.cc Modified: code/player/trunk/server/drivers/planner/wavefront/plan_control.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-04-16 09:10:27 UTC (rev 6335) +++ code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-04-16 17:23:26 UTC (rev 6336) @@ -119,18 +119,18 @@ { if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->abs_min_radius) return -1; - else + else if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->max_radius) obscost += plan->dist_penalty * - (plan->abs_min_radius - + (plan->max_radius - plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn); } else { if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn < plan->abs_min_radius) return -1; - else + else if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn < plan->max_radius) obscost += plan->dist_penalty * - (plan->abs_min_radius - + (plan->max_radius - plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn); } @@ -148,18 +148,18 @@ { if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->abs_min_radius) return -1; - else + else if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->max_radius) obscost += plan->dist_penalty * - (plan->abs_min_radius - + (plan->max_radius - plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn); } else { if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn < plan->abs_min_radius) return -1; - else + else if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn < plan->max_radius) obscost += plan->dist_penalty * - (plan->abs_min_radius - + (plan->max_radius - plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn); } } Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-16 09:10:27 UTC (rev 6335) +++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-16 17:23:26 UTC (rev 6336) @@ -645,15 +645,8 @@ t1 = get_time(); printf("ProcessLaserScan: %.6lf\n", t1-t0); -#if 0 if(this->graphics2d_id.interf) { - // Clear the canvas - this->graphics2d->PutMsg(this->InQueue, - PLAYER_MSGTYPE_CMD, - PLAYER_GRAPHICS2D_CMD_CLEAR, - NULL,0,NULL); - // Draw the points player_graphics2d_cmd_points pts; assert(pts.points = (player_point_2d_t*)malloc(sizeof(player_point_2d_t)* @@ -675,7 +668,6 @@ (void*)&pts,0,NULL); free(pts.points); } -#endif } void @@ -962,6 +954,14 @@ this->new_map = false; } #endif + if(this->graphics2d_id.interf) + { + this->graphics2d->PutMsg(this->InQueue, + PLAYER_MSGTYPE_CMD, + PLAYER_GRAPHICS2D_CMD_CLEAR, + NULL,0,NULL); + } + double t0,t1; t0 = get_time(); @@ -985,11 +985,6 @@ if(this->graphics2d_id.interf && this->plan->lpath_count) { - this->graphics2d->PutMsg(this->InQueue, - PLAYER_MSGTYPE_CMD, - PLAYER_GRAPHICS2D_CMD_CLEAR, - NULL,0,NULL); - player_graphics2d_cmd_polyline_t line; line.points_count = this->plan->lpath_count; line.points = (player_point_2d_t*)malloc(sizeof(player_point_2d_t)*line.points_count); @@ -1110,6 +1105,27 @@ } else { + if(this->graphics2d_id.interf) + { + player_graphics2d_cmd_polyline_t line; + line.points_count = 2; + line.points = (player_point_2d_t*)malloc(sizeof(player_point_2d_t)*line.points_count); + line.color.alpha = 0; + line.color.red = 0; + line.color.green = 0; + line.color.blue = 255; + + line.points[0].px = this->localize_x; + line.points[0].py = this->localize_y; + line.points[1].px = wx; + line.points[1].py = wy; + this->graphics2d->PutMsg(this->InQueue, + PLAYER_MSGTYPE_CMD, + PLAYER_GRAPHICS2D_CMD_POLYLINE, + (void*)&line,0,NULL); + free(line.points); + } + // Establish fake waypoints, for client-side visualization this->curr_waypoint = 0; this->waypoint_count = 2; @@ -1123,7 +1139,7 @@ double tvmin = 0.1; double tvmax = 0.5; double avmin = DTOR(10.0); - double avmax = DTOR(45.0); + double avmax = DTOR(90.0); double amin = DTOR(5.0); double amax = DTOR(20.0); @@ -1156,7 +1172,10 @@ if(fabs(ad) > a) tv = 0.0; else - tv = tvmin + (d / (M_SQRT2 * maxd)) * (tvmax-tvmin); + { + //tv = tvmin + (d / (M_SQRT2 * maxd)) * (tvmax-tvmin); + tv = tvmin + (d / maxd) * (tvmax-tvmin); + } av = avmin + (fabs(ad)/M_PI) * (avmax-avmin); if(ad < 0) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-04-16 10:59:31
|
Revision: 6339 http://playerstage.svn.sourceforge.net/playerstage/?rev=6339&view=rev Author: gerkey Date: 2008-04-16 10:59:37 -0700 (Wed, 16 Apr 2008) Log Message: ----------- working on timing Modified Paths: -------------- code/player/trunk/server/drivers/planner/wavefront/Makefile.am code/player/trunk/server/drivers/planner/wavefront/plan.c code/player/trunk/server/drivers/planner/wavefront/plan_control.c code/player/trunk/server/drivers/planner/wavefront/plan_plan.c code/player/trunk/server/drivers/planner/wavefront/wavefront.cc Modified: code/player/trunk/server/drivers/planner/wavefront/Makefile.am =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/Makefile.am 2008-04-16 17:24:32 UTC (rev 6338) +++ code/player/trunk/server/drivers/planner/wavefront/Makefile.am 2008-04-16 17:59:37 UTC (rev 6339) @@ -3,7 +3,7 @@ noinst_LTLIBRARIES += libwavefront.la endif -AM_CPPFLAGS = -Wall -g -O3 -I$(top_srcdir) `pkg-config --cflags gdk-pixbuf-2.0` +AM_CPPFLAGS = -Wall -g -I$(top_srcdir) `pkg-config --cflags gdk-pixbuf-2.0` -O3 libwavefront_la_SOURCES = plan.c plan_plan.c plan_waypoint.c plan.h \ plan_control.c wavefront.cc heap.h heap.c Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-16 17:24:32 UTC (rev 6338) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-16 17:59:37 UTC (rev 6339) @@ -135,7 +135,7 @@ } t1 = get_time(); - printf("plan_set_obstacles: %.6lf\n", t1-t0); + //printf("plan_set_obstacles: %.6lf\n", t1-t0); } void Modified: code/player/trunk/server/drivers/planner/wavefront/plan_control.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-04-16 17:24:32 UTC (rev 6338) +++ code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-04-16 17:59:37 UTC (rev 6339) @@ -41,8 +41,8 @@ // Check whether the straight-line path is clear if((cost = _plan_check_path(plan, cell, ncell)) < 0.0) { - printf("no path from (%d,%d) to (%d,%d)\n", - cell->ci, cell->cj, ncell->ci, ncell->cj); + //printf("no path from (%d,%d) to (%d,%d)\n", + //cell->ci, cell->cj, ncell->ci, ncell->cj); continue; } Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-16 17:24:32 UTC (rev 6338) +++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-16 17:59:37 UTC (rev 6339) @@ -67,7 +67,7 @@ t1 = get_time(); - printf("computed global path: %.6lf\n", t1-t0); + //printf("computed global path: %.6lf\n", t1-t0); return(0); } @@ -136,7 +136,7 @@ t1 = get_time(); - printf("computed local path: %.6lf\n", t1-t0); + //printf("computed local path: %.6lf\n", t1-t0); return(0); } Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-16 17:24:32 UTC (rev 6338) +++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-16 17:59:37 UTC (rev 6339) @@ -588,7 +588,7 @@ this->scans_count = MIN(this->scans_count,this->scans_size); this->scans_idx = (this->scans_idx + 1) % this->scans_size; - printf("%d scans\n", this->scans_count); + //printf("%d scans\n", this->scans_count); // run through the scans to see how much room we need to store all the // hitpoints @@ -639,11 +639,11 @@ } } - printf("setting %d hit points\n", this->scan_points_count); + //printf("setting %d hit points\n", this->scan_points_count); plan_set_obstacles(plan, this->scan_points, this->scan_points_count); t1 = get_time(); - printf("ProcessLaserScan: %.6lf\n", t1-t0); + //printf("ProcessLaserScan: %.6lf\n", t1-t0); if(this->graphics2d_id.interf) { @@ -857,6 +857,7 @@ bool rotate_waypoint=false; bool replan; int rotate_dir=0; + double m0=0.0,m1; pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL); @@ -867,17 +868,25 @@ for(;;) { + GlobalTime->GetTimeDouble(&t); + + m1 = get_time(); + printf("loop time: %.6lf\n", m1-m0); + m0 = get_time(); + pthread_testcancel(); ProcessMessages(); if(!this->have_map && !this->new_map_available) { - usleep(CYCLE_TIME_US); + double currt,tdiff; + GlobalTime->GetTimeDouble(&currt); + tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6); + usleep((unsigned int)rint(tdiff)); continue; } - GlobalTime->GetTimeDouble(&t); if((t - last_publish_time) > 0.25) { @@ -888,7 +897,11 @@ if(!this->enable) { this->StopPosition(); - usleep(CYCLE_TIME_US); + double currt,tdiff; + GlobalTime->GetTimeDouble(&currt); + tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6); + printf("sleeping: %u\n", (unsigned int)rint(tdiff)); + usleep((unsigned int)rint(tdiff)); continue; } @@ -1004,8 +1017,29 @@ free(line.points); } + if(this->graphics2d_id.interf && this->plan->path_count) + { + player_graphics2d_cmd_polyline_t line; + line.points_count = this->plan->path_count; + line.points = (player_point_2d_t*)malloc(sizeof(player_point_2d_t)*line.points_count); + line.color.alpha = 0; + line.color.red = 255; + line.color.green = 0; + line.color.blue = 0; + for(int i=0;i<this->plan->path_count;i++) + { + line.points[i].px = PLAN_WXGX(this->plan,this->plan->path[i]->ci); + line.points[i].py = PLAN_WYGY(this->plan,this->plan->path[i]->cj); + } + this->graphics2d->PutMsg(this->InQueue, + PLAYER_MSGTYPE_CMD, + PLAYER_GRAPHICS2D_CMD_POLYLINE, + (void*)&line,0,NULL); + free(line.points); + } + t1 = get_time(); - printf("planning: %.6lf\n", t1-t0); + //printf("planning: %.6lf\n", t1-t0); if(!this->velocity_control) { @@ -1092,8 +1126,8 @@ double maxd=2.0; double distweight=10.0; - printf("pose: (%.3lf,%.3lf,%.3lf)\n", - this->localize_x, this->localize_y, RTOD(this->localize_a)); + //printf("pose: (%.3lf,%.3lf,%.3lf)\n", + //this->localize_x, this->localize_y, RTOD(this->localize_a)); if(plan_get_carrot(this->plan, &wx, &wy, this->localize_x, this->localize_y, maxd, distweight) < 0) @@ -1143,6 +1177,10 @@ double amin = DTOR(5.0); double amax = DTOR(20.0); + double goald = sqrt((this->localize_x-this->target_x)* + (this->localize_x-this->target_x) + + (this->localize_y-this->target_y)* + (this->localize_y-this->target_y)); double d = sqrt((this->localize_x-wx)*(this->localize_x-wx) + (this->localize_y-wy)*(this->localize_y-wy)); double b = atan2(wy - this->localize_y, wx - this->localize_x); @@ -1152,7 +1190,7 @@ double ad = angle_diff(b, this->localize_a); // Are we on top of the goal? - if(d < this->dist_eps) + if(goald < this->dist_eps) { if(!rotate_dir) { @@ -1190,7 +1228,7 @@ this->StopPosition(); t1 = get_time(); - printf("control: %.6lf\n", t1-t0); + //printf("control: %.6lf\n", t1-t0); } else // !velocity_control { @@ -1243,7 +1281,10 @@ // no more waypoints, so wait for target achievement //puts("waiting for goal achievement"); - usleep(CYCLE_TIME_US); + double currt,tdiff; + GlobalTime->GetTimeDouble(&currt); + tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6); + usleep((unsigned int)rint(tdiff)); continue; } // get next waypoint @@ -1283,7 +1324,11 @@ } } - usleep(CYCLE_TIME_US); + double currt,tdiff; + GlobalTime->GetTimeDouble(&currt); + tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6); + usleep((unsigned int)rint(tdiff)); + printf("sleeping: %u\n", (unsigned int)rint(tdiff)); } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-04-16 12:40:01
|
Revision: 6341 http://playerstage.svn.sourceforge.net/playerstage/?rev=6341&view=rev Author: gerkey Date: 2008-04-16 12:40:05 -0700 (Wed, 16 Apr 2008) Log Message: ----------- final debugging Modified Paths: -------------- code/player/trunk/server/drivers/planner/wavefront/plan.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-16 17:59:52 UTC (rev 6340) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-16 19:40:05 UTC (rev 6341) @@ -193,6 +193,8 @@ int i, j; plan_cell_t *cell; + printf("scale: %.3lf\n", plan->scale); + cell = plan->cells; for (j = 0; j < plan->size_y; j++) { Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-16 17:59:52 UTC (rev 6340) +++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-16 19:40:05 UTC (rev 6341) @@ -229,15 +229,14 @@ // TODO: monitor localize timestamps, and slow or stop robot accordingly -// time to sleep between loops (us) -#define CYCLE_TIME_US 100000 - class Wavefront : public Driver { private: // Main function for device thread. virtual void Main(); + void Sleep(double loopstart); + // bookkeeping player_devaddr_t position_id; player_devaddr_t localize_id; @@ -251,7 +250,8 @@ double dist_penalty; double dist_eps; double ang_eps; - const char* cspace_fname; + double cycletime; + double tvmin, tvmax, avmin, avmax, amin, amax; // the plan object plan_t* plan; @@ -422,11 +422,22 @@ this->replan_dist_thresh = cf->ReadLength(section,"replan_dist_thresh",2.0); this->replan_min_time = cf->ReadFloat(section,"replan_min_time",2.0); this->request_map = cf->ReadInt(section,"request_map",1); - this->cspace_fname = cf->ReadFilename(section,"cspace_file","player.cspace"); this->always_insert_rotational_waypoints = cf->ReadInt(section, "add_rotational_waypoints", 1); this->force_map_refresh = cf->ReadInt(section, "force_map_refresh", 0); + this->cycletime = 1.0 / cf->ReadFloat(section, "update_rate", 10.0); + this->velocity_control = cf->ReadInt(section, "velocity_control", 0); + if(this->velocity_control) + { + this->tvmin = cf->ReadTupleLength(section, "control_tv", 0, 0.1); + this->tvmax = cf->ReadTupleLength(section, "control_tv", 1, 0.5); + this->avmin = cf->ReadTupleAngle(section, "control_av", 0, DTOR(10.0)); + this->avmax = cf->ReadTupleAngle(section, "control_av", 1, DTOR(90.0)); + this->amin = cf->ReadTupleAngle(section, "control_a", 0, DTOR(5.0)); + this->amax = cf->ReadTupleAngle(section, "control_a", 1, DTOR(20.0)); + } + if(this->laser_id.interf) { this->scans_size = cf->ReadInt(section, "laser_buffer_size", 10); @@ -438,7 +449,11 @@ this->scan_maxrange = cf->ReadLength(section, "laser_maxrange", 6.0); } else + { this->scans_size = 0; + if(this->velocity_control) + PLAYER_WARN("Wavefront doing direct velocity control, but without a laser for obstacle detection; this is not safe!"); + } } @@ -843,7 +858,20 @@ this->waypoint_odom_a = wa_odom; } +void +Wavefront::Sleep(double loopstart) +{ + double currt,tdiff; + //GlobalTime->GetTimeDouble(&currt); + currt = get_time(); + //printf("cycle: %.6lf\n", currt-loopstart); + tdiff = MAX(0.0, this->cycletime - (currt-loopstart)); + if(tdiff == 0.0) + PLAYER_WARN("Wavefront missed deadline and not sleeping; check machine load"); + usleep((unsigned int)rint(tdiff)); +} + //////////////////////////////////////////////////////////////////////////////// // Main function for device thread void Wavefront::Main() @@ -857,7 +885,6 @@ bool rotate_waypoint=false; bool replan; int rotate_dir=0; - double m0=0.0,m1; pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL); @@ -868,22 +895,16 @@ for(;;) { - GlobalTime->GetTimeDouble(&t); + //GlobalTime->GetTimeDouble(&t); + t = get_time(); - m1 = get_time(); - printf("loop time: %.6lf\n", m1-m0); - m0 = get_time(); - pthread_testcancel(); ProcessMessages(); if(!this->have_map && !this->new_map_available) { - double currt,tdiff; - GlobalTime->GetTimeDouble(&currt); - tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6); - usleep((unsigned int)rint(tdiff)); + this->Sleep(t); continue; } @@ -897,11 +918,7 @@ if(!this->enable) { this->StopPosition(); - double currt,tdiff; - GlobalTime->GetTimeDouble(&currt); - tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6); - printf("sleeping: %u\n", (unsigned int)rint(tdiff)); - usleep((unsigned int)rint(tdiff)); + this->Sleep(t); continue; } @@ -1169,14 +1186,6 @@ this->waypoint_y = this->waypoints[1][1] = wy; this->waypoint_a = 0.0; - // TODO: expose these control params in the .cfg file - double tvmin = 0.1; - double tvmax = 0.5; - double avmin = DTOR(10.0); - double avmax = DTOR(90.0); - double amin = DTOR(5.0); - double amax = DTOR(20.0); - double goald = sqrt((this->localize_x-this->target_x)* (this->localize_x-this->target_x) + (this->localize_y-this->target_y)* @@ -1186,7 +1195,7 @@ double b = atan2(wy - this->localize_y, wx - this->localize_x); double av,tv; - double a = amin + (d / maxd) * (amax-amin); + double a = this->amin + (d / maxd) * (this->amax-this->amin); double ad = angle_diff(b, this->localize_a); // Are we on top of the goal? @@ -1201,7 +1210,8 @@ } tv = 0.0; - av = rotate_dir * (avmin + (fabs(ad)/M_PI) * (avmax-avmin)); + av = rotate_dir * (this->avmin + (fabs(ad)/M_PI) * + (this->avmax-this->avmin)); } else { @@ -1212,10 +1222,10 @@ else { //tv = tvmin + (d / (M_SQRT2 * maxd)) * (tvmax-tvmin); - tv = tvmin + (d / maxd) * (tvmax-tvmin); + tv = this->tvmin + (d / maxd) * (this->tvmax-this->tvmin); } - av = avmin + (fabs(ad)/M_PI) * (avmax-avmin); + av = this->avmin + (fabs(ad)/M_PI) * (this->avmax-this->avmin); if(ad < 0) av = -av; } @@ -1281,10 +1291,7 @@ // no more waypoints, so wait for target achievement //puts("waiting for goal achievement"); - double currt,tdiff; - GlobalTime->GetTimeDouble(&currt); - tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6); - usleep((unsigned int)rint(tdiff)); + this->Sleep(t); continue; } // get next waypoint @@ -1324,11 +1331,7 @@ } } - double currt,tdiff; - GlobalTime->GetTimeDouble(&currt); - tdiff = MAX(0.0, (CYCLE_TIME_US) - (currt-t)*1e6); - usleep((unsigned int)rint(tdiff)); - printf("sleeping: %u\n", (unsigned int)rint(tdiff)); + this->Sleep(t); } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-04-18 18:58:28
|
Revision: 6367 http://playerstage.svn.sourceforge.net/playerstage/?rev=6367&view=rev Author: gerkey Date: 2008-04-18 18:58:34 -0700 (Fri, 18 Apr 2008) Log Message: ----------- took out debug statements Modified Paths: -------------- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c code/player/trunk/server/drivers/planner/wavefront/wavefront.cc Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-18 22:50:59 UTC (rev 6366) +++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-04-19 01:58:34 UTC (rev 6367) @@ -249,7 +249,7 @@ if(!cell->plan_next) { - puts("never found start"); + //puts("never found start"); return(-1); } else Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-18 22:50:59 UTC (rev 6366) +++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-04-19 01:58:34 UTC (rev 6367) @@ -868,7 +868,7 @@ tdiff = MAX(0.0, this->cycletime - (currt-loopstart)); if(tdiff == 0.0) PLAYER_WARN("Wavefront missed deadline and not sleeping; check machine load"); - usleep((unsigned int)rint(tdiff)); + usleep((unsigned int)rint(tdiff*1e6)); } @@ -885,6 +885,7 @@ bool rotate_waypoint=false; bool replan; int rotate_dir=0; + bool printed_warning=false; pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL); @@ -1003,14 +1004,23 @@ this->localize_y, this->scan_maxrange) < 0)) { if(!new_goal && (this->plan->path_count != 0)) - puts("Wavefront: local plan failed"); + puts("Wavefront: local plan failed"); // Create a global plan if(plan_do_global(this->plan, this->localize_x, this->localize_y, this->target_x, this->target_y) < 0) - puts("Wavefront: global plan failed"); + { + if(!printed_warning) + { + puts("Wavefront: global plan failed"); + printed_warning = true; + } + } else + { this->new_goal = false; + printed_warning = false; + } } if(this->graphics2d_id.interf && this->plan->lpath_count) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-05-01 17:17:13
|
Revision: 6392 http://playerstage.svn.sourceforge.net/playerstage/?rev=6392&view=rev Author: gerkey Date: 2008-05-01 17:17:20 -0700 (Thu, 01 May 2008) Log Message: ----------- updated test Modified Paths: -------------- 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/test.c Modified: code/player/trunk/server/drivers/planner/wavefront/Makefile.test =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/Makefile.test 2008-04-29 18:58:10 UTC (rev 6391) +++ code/player/trunk/server/drivers/planner/wavefront/Makefile.test 2008-05-02 00:17:20 UTC (rev 6392) @@ -1,6 +1,6 @@ #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` +LIBS = -L../../../../build/libplayercore -lplayererror `pkg-config --libs gdk-pixbuf-2.0` SRCS = test.c plan.c plan_plan.c plan_waypoint.c heap.c plan_control.c Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-04-29 18:58:10 UTC (rev 6391) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-05-02 00:17:20 UTC (rev 6392) @@ -6,7 +6,7 @@ * CVS: $Id$ **************************************************************************/ -#include <config.h> +//#include <config.h> // This header MUST come before <openssl/md5.h> #include <sys/types.h> Modified: code/player/trunk/server/drivers/planner/wavefront/test.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/test.c 2008-04-29 18:58:10 UTC (rev 6391) +++ code/player/trunk/server/drivers/planner/wavefront/test.c 2008-05-02 00:17:20 UTC (rev 6392) @@ -59,7 +59,7 @@ assert((plan = plan_alloc(robot_radius+safety_dist, robot_radius+safety_dist, max_radius, - dist_penalty))); + dist_penalty,0.5))); // allocate space for map cells assert(plan->cells == NULL); @@ -187,8 +187,8 @@ 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++; + //if(gdk_pixbuf_get_has_alpha(pixbuf)) + //n_channels++; // Read data pixels = gdk_pixbuf_get_pixels(pixbuf); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-05-07 09:38:56
|
Revision: 6399 http://playerstage.svn.sourceforge.net/playerstage/?rev=6399&view=rev Author: gerkey Date: 2008-05-07 09:39:01 -0700 (Wed, 07 May 2008) Log Message: ----------- added build of standalone planning library; once stable, it will likely migrate to Gearbox Modified Paths: -------------- code/player/trunk/server/drivers/planner/wavefront/CMakeLists.txt code/player/trunk/server/drivers/planner/wavefront/plan.c code/player/trunk/server/drivers/planner/wavefront/plan_waypoint.c Modified: code/player/trunk/server/drivers/planner/wavefront/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/CMakeLists.txt 2008-05-04 02:12:02 UTC (rev 6398) +++ code/player/trunk/server/drivers/planner/wavefront/CMakeLists.txt 2008-05-07 16:39:01 UTC (rev 6399) @@ -3,4 +3,8 @@ 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 heap.c plan_control.c) \ No newline at end of file + plan.c plan_plan.c plan_waypoint.c wavefront.cc heap.c plan_control.c) + +# Also build and install standalone non-Player lib +PLAYER_ADD_LIBRARY (wavefront_standalone plan.c plan_plan.c plan_waypoint.c heap.c plan_control.c) +PLAYER_INSTALL_HEADERS (standalone_drivers plan.h heap.h) Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-05-04 02:12:02 UTC (rev 6398) +++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2008-05-07 16:39:01 UTC (rev 6399) @@ -350,7 +350,7 @@ float* p; plan_cell_t *cell, *ncell; - PLAYER_MSG0(2,"Generating C-space...."); + puts("Generating C-space...."); for (j = plan->min_y; j <= plan->max_y; j++) { @@ -381,6 +381,7 @@ } } +#if 0 #include <gdk-pixbuf/gdk-pixbuf.h> void @@ -541,6 +542,7 @@ gdk_pixbuf_unref(pixbuf); free(pixels); } +#endif // Construct the configuration space from the occupancy grid. // This treats both occupied and unknown cells as bad. @@ -585,7 +587,6 @@ 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. @@ -717,6 +718,8 @@ } #endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO +#endif // if 0 + double static get_time(void) { Modified: code/player/trunk/server/drivers/planner/wavefront/plan_waypoint.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_waypoint.c 2008-05-04 02:12:02 UTC (rev 6398) +++ code/player/trunk/server/drivers/planner/wavefront/plan_waypoint.c 2008-05-07 16:39:01 UTC (rev 6399) @@ -129,7 +129,7 @@ lastj = (int)floor(j); if(!PLAN_VALID(plan,lasti,lastj)) { - PLAYER_WARN("stepped off the map!"); + //PLAYER_WARN("stepped off the map!"); return(0); } if(plan->cells[PLAN_INDEX(plan,lasti,lastj)].occ_dist < This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-05-07 19:01:19
|
Revision: 6400 http://playerstage.svn.sourceforge.net/playerstage/?rev=6400&view=rev Author: gerkey Date: 2008-05-07 19:01:26 -0700 (Wed, 07 May 2008) Log Message: ----------- added more to low-level controller Modified Paths: -------------- code/player/trunk/server/drivers/planner/wavefront/plan.h code/player/trunk/server/drivers/planner/wavefront/plan_control.c code/player/trunk/server/drivers/planner/wavefront/wavefront.cc Modified: code/player/trunk/server/drivers/planner/wavefront/plan.h =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-05-07 16:39:01 UTC (rev 6399) +++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2008-05-08 02:01:26 UTC (rev 6400) @@ -150,6 +150,19 @@ double plan_get_carrot(plan_t* plan, double* px, double* py, double lx, double ly, double maxdist, double distweight); +int plan_compute_diffdrive_cmds(plan_t* plan, double* vx, double *va, + int* rotate_dir, + double lx, double ly, double la, + double gx, double gy, double ga, + double goal_d, double goal_a, + double maxd, double dweight, + double tvmin, double tvmax, + double avmin, double avmax, + double amin, double amax); +int plan_check_done(plan_t* plan, + double lx, double ly, double la, + double gx, double gy, double ga, + double goal_d, double goal_a); void plan_set_obstacles(plan_t* plan, double* obs, size_t num); Modified: code/player/trunk/server/drivers/planner/wavefront/plan_control.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-05-07 16:39:01 UTC (rev 6399) +++ code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-05-08 02:01:26 UTC (rev 6400) @@ -1,10 +1,85 @@ #include <stdlib.h> +#include <math.h> #include <assert.h> #include "plan.h" -double _plan_check_path(plan_t* plan, plan_cell_t* s, plan_cell_t* g); +static double _plan_check_path(plan_t* plan, plan_cell_t* s, plan_cell_t* g); +static double _angle_diff(double a, double b); +int +plan_check_done(plan_t* plan, + double lx, double ly, double la, + double gx, double gy, double ga, + double goal_d, double goal_a) +{ + double dt, da; + dt = sqrt((gx-lx)*(gx-ly) + (gy-ly)*(gy-ly)); + da = fabs(_angle_diff(ga,la)); + + if((dt < goal_d) && (da < goal_a)) + return(1); + else + return(0); +} + +int +plan_compute_diffdrive_cmds(plan_t* plan, double* vx, double *va, + int* rotate_dir, + double lx, double ly, double la, + double gx, double gy, double ga, + double goal_d, double goal_a, + double maxd, double dweight, + double tvmin, double tvmax, + double avmin, double avmax, + double amin, double amax) +{ + double cx, cy; + double d,b,av,tv,a,ad; + + // Are we at the goal? + if(plan_check_done(plan,lx,ly,la,gx,gy,ga,goal_d,goal_a)) + { + *vx = 0.0; + *va = 0.0; + return(0); + } + + // Are we on top of the goal? + d = sqrt((gx-lx)*(gx-lx)+(gy-ly)*(gy-ly)); + if(d < goal_d) + { + ad = _angle_diff(ga,la); + if(!*rotate_dir) + { + if(ad < 0) + *rotate_dir = -1; + else + *rotate_dir = 1; + } + *vx = 0.0; + *va = *rotate_dir * (avmin + (fabs(ad)/M_PI) * (avmax-avmin)); + return(0); + } + + // We're away from the goal; compute velocities + if(plan_get_carrot(plan, &cx, &cy, lx, ly, maxd, dweight) < 0.0) + return(-1); + + d = sqrt((lx-cx)*(lx-cx) + (ly-cy)*(ly-cy)); + b = atan2(cy - ly, cx - lx); + a = amin + (d / maxd) * (amax-amin); + ad = _angle_diff(b, la); + + if(fabs(ad) > a) + *vx = 0.0; + else + tv = tvmin + (d / maxd) * (tvmax-tvmin); + av = avmin + (fabs(ad)/M_PI) * (avmax-avmin); + if(ad < 0) + av = -av; +} + double plan_get_carrot(plan_t* plan, double* px, double* py, double lx, double ly, double maxdist, double distweight) @@ -64,7 +139,7 @@ return(bestcost); } -double +static double _plan_check_path(plan_t* plan, plan_cell_t* s, plan_cell_t* g) { // Bresenham raytracing @@ -167,4 +242,20 @@ return(obscost); } +#define ANG_NORM(a) atan2(cos((a)),sin((a))) +static double +_angle_diff(double a, double b) +{ + double d1, d2; + a = ANG_NORM(a); + b = ANG_NORM(b); + d1 = a-b; + d2 = 2*M_PI - fabs(d1); + if(d1 > 0) + d2 *= -1.0; + if(fabs(d1) < fabs(d2)) + return(d1); + else + return(d2); +} Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-05-07 16:39:01 UTC (rev 6399) +++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc 2008-05-08 02:01:26 UTC (rev 6400) @@ -225,7 +225,7 @@ #include <sys/time.h> static double get_time(void); -extern "C" { void draw_cspace(plan_t* plan, const char* fname); } +//extern "C" { void draw_cspace(plan_t* plan, const char* fname); } // TODO: monitor localize timestamps, and slow or stop robot accordingly @@ -1542,7 +1542,7 @@ plan_init(this->plan); plan_compute_cspace(this->plan); - draw_cspace(this->plan,"cspace.png"); + //draw_cspace(this->plan,"cspace.png"); return(0); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ge...@us...> - 2008-07-09 18:34:48
|
Revision: 6833 http://playerstage.svn.sourceforge.net/playerstage/?rev=6833&view=rev Author: gerkey Date: 2008-07-09 18:34:50 -0700 (Wed, 09 Jul 2008) Log Message: ----------- fixed turn in place bug, and removed verbose debug output Modified Paths: -------------- code/player/trunk/server/drivers/planner/wavefront/plan_control.c code/player/trunk/server/drivers/planner/wavefront/plan_plan.c Modified: code/player/trunk/server/drivers/planner/wavefront/plan_control.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-07-10 00:12:25 UTC (rev 6832) +++ code/player/trunk/server/drivers/planner/wavefront/plan_control.c 2008-07-10 01:34:50 UTC (rev 6833) @@ -67,6 +67,8 @@ return(0); } + *rotate_dir = 0; + // We're away from the goal; compute velocities if(plan_get_carrot(plan, &cx, &cy, lx, ly, maxd, dweight) < 0.0) { Modified: code/player/trunk/server/drivers/planner/wavefront/plan_plan.c =================================================================== --- code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-07-10 00:12:25 UTC (rev 6832) +++ code/player/trunk/server/drivers/planner/wavefront/plan_plan.c 2008-07-10 01:34:50 UTC (rev 6833) @@ -97,7 +97,7 @@ // Find a local goal to pursue if(_plan_find_local_goal(plan, &gx, &gy, lx, ly) != 0) { - puts("no local goal"); + //puts("no local goal"); return(-1); } @@ -276,7 +276,7 @@ // Must already have computed a global goal if(plan->path_count == 0) { - puts("no global path"); + //puts("no global path"); return(-1); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |