Add a simple test for A* algorithm

This is meant for Matous to test his new planning algorithm.

diff --git a/src/pathplan/test/Makefile.omk b/src/pathplan/test/Makefile.omk
index 59e92006fd181af74dee5eda2059acc7e4f2bf10..71fa8470651fd83f7da050fe6a8e6212baa322cc 100644 (file)
--- a/src/pathplan/test/Makefile.omk
+++ b/src/pathplan/test/Makefile.omk
@@ -7,3 +7,8 @@ testmap_LIBS = map m
 test_PROGRAMS += testpathplan
 testpathplan_SOURCES = testpathplan.c
 testpathplan_LIBS = pathplan rt map m rbtree shist
+
+test_PROGRAMS += testastar
+testastar_SOURCES = testastar.c
+testastar_LIBS = pathplan map m 
+

diff --git a/src/pathplan/test/testastar.c b/src/pathplan/test/testastar.c
new file mode 100644 (file)
index 0000000..8651bd3
--- /dev/null
+++ b/src/pathplan/test/testastar.c
@@ -0,0 +1,134 @@
+#define _ISOC99_SOURCE
+#include <stdio.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include "path_planner.h"
+#include <time.h>
+#include <sys/time.h>
+#include <math.h>
+#include <shist.h>
+
+#define OBST_COUNT 4
+
+void print_map(char *label)
+{
+       int x, y;
+       struct map *map = ShmapIsMapInit();
+
+       if (label)
+               printf("%s\n", label);
+       
+       for (y=0; y<MAP_HEIGHT-1; y++) {
+               for (x=0; x<MAP_WIDTH; x++) {
+                       char c;
+                       if (ShmapIsFreeCell(x, y)) {
+                               c = '.';
+                               if (map->cells[y][x].flags & MAP_FLAG_PATH) {
+                                       c = 'p';
+                               }
+                       } else
+                               c = '#';
+                       putchar(c);
+               }
+               putchar('\n');
+       }
+}
+
+void randomize_obstacles(bool first)
+{
+        static struct obstacle {
+                double x1, y1, x2, y2;
+        } o[OBST_COUNT];
+        int i;
+        if (!first) {
+                for (i=0; i<OBST_COUNT; i++) {
+                        ShmapSetRectangleFlag(o[i].x1, o[i].y1, o[i].x2, o[i].y2, 0, MAP_FLAG_WALL);
+                }
+        }
+        for (i=0; i<OBST_COUNT; i++) {
+                double x, y, w, h;
+                x = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
+               y = (rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0;
+                w = (rand()%1000)/1000.0;
+                h = (rand()%1000)/1000.0;
+                o[i].x1 = x-w/2;
+               o[i].y1 = y-h/2;
+                o[i].x2 = x+w/2;
+               o[i].y2 = y+h/2;
+/*                 printf("Obst%d (%5.2f,%5.2f) (%5.2f,%5.2f)\n", i, o[i].x1, o[i].y1, o[i].x2, o[i].y2); */
+/*                 printf("Obst%d (%5.2f,%5.2f) w=%5.2f  h=%5.2f\n", i, x, y, w, h); */
+                ShmapSetRectangleFlag(o[i].x1, o[i].y1, o[i].x2, o[i].y2, MAP_FLAG_WALL, 0);
+        }
+       print_map("Obstacles:");
+}
+
+    
+
+int main(int argc, char *argv[])
+{
+       double startx, starty, goalx, goaly, angle;
+       PathPoint * path;
+       int i, val;
+       FILE * data =NULL;
+
+       srand(time(0));
+       
+       /* histogram parameters */
+       if (argc == 2) {
+               data = fopen( argv[1], "w" );
+               if( data )
+                       printf( "Storing data at file : %s\n", argv[1] );
+               else
+               {
+                       printf( "Open file failed : %s\n", argv[1] );
+                       return 1;
+               }
+       }
+       
+       // Init Shared Map Memory
+       ShmapInit(1);
+       
+       // Create some obstacles
+       ShmapSetRectangleFlag(0.0, 0.0, 0.2, 2.1, MAP_FLAG_WALL, 0);
+       ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.2, MAP_FLAG_WALL, 0);
+       ShmapSetRectangleFlag(0.0, 2.0, 3.0, 2.1, MAP_FLAG_WALL, 0);
+       ShmapSetRectangleFlag(2.9, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0);
+        randomize_obstacles(true);
+        do {
+                startx = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
+                starty = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0);
+        } while (!ShmapIsFreePoint(startx, starty));
+        
+       /* Main boucle */
+       for(i=1; i<2; i++){
+                if (i%100 == 0) {
+                        randomize_obstacles(false);
+                }
+               do {
+                       goalx = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
+                       goaly = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0);
+               } while (!ShmapIsFreePoint(goalx, goaly));
+               
+               val = path_planner(startx, starty, goalx, goaly , &path, &angle);
+
+               print_map("Found path:"); 
+               
+               startx=goalx;starty=goaly;
+       }
+       
+       //Close File
+       if (data){
+               if( !fclose(data) )
+                       printf( "Data file closed : %s\n", argv[1] );
+               else
+               {
+                       printf( "Error: file not closed : %s\n", argv[1] );
+                       return 1;
+               }
+       }
+       // Free Memory
+       //ShmapFree();
+       return 0;
+}
+
+