#include <sys/time.h>
#include <math.h>
#include <shist.h>
+#include "aalgorithm.h"
#define OBST_COUNT 4
+#define NBR_OF_TESTS 2
+#define DBG_PNG
+#ifdef DBG_PNG
+#include "map_2_png.h"
+#endif
+
+// print map to console
void print_map(char *label)
{
int x, y;
/* 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:");
+ //print_map("Obstacles:");
}
void set_fix_obstacle(double x1,double y1,double x2,double y2){
- ShmapSetRectangleFlag(x1, x2, y1, y2, MAP_FLAG_WALL, 0);
+ ShmapSetRectangleFlag(x1, y1, x2, y2, MAP_FLAG_WALL, 0);
}
int main(int argc, char *argv[])
{
- double startx, starty, goalx, goaly, angle;
+ double startx_real, starty_real, goalx_real, goaly_real, angle = 0;
+ int startx = 0, starty = 0, goalx = 0, goaly = 0;
PathPoint * path;
int i, val;
FILE * data =NULL;
-
+
+ init_aalgorithm();
+
srand(time(0));
/* histogram parameters */
}
// Init Shared Map Memory
- ShmapInit(1);
-
+ struct map *map;
+ map = ShmapInit(1);
+
// Create some obstacles
- ShmapSetRectangleFlag(0.0, 0.0, 0.1, 2.1, MAP_FLAG_WALL, 0);
- ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.1, 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);
+ ShmapSetRectangleFlag(1.0, 0.0, 1.05, 1.45, MAP_FLAG_WALL, 0);
+ ShmapSetRectangleFlag(1.55, 0.65, 1.60, 2.1, MAP_FLAG_WALL, 0);
-// set_fix_obstacle(0.3, 0.3, 0.8, 0.8);
-// set_fix_obstacle(1.7, 0.5, 2.2, 1.2);
-// set_fix_obstacle(1.1, 1.1, 1.3, 1.33);
+ // Add safety margin manualy, remove add_safety_margin (path_planer.c)
+// ShmapSetRectangleFlag(0.0, 0.0, 0.99, 0.27, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.08, 0.0, 3.0, 0.27, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(0.0, 0.0, 0.27, 2.1, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(0.0, 1.87, 1.54, 2.1, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.63, 1.87, 3.0, 2.1, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(2.73, 0.0, 3.0, 2.1, MAP_FLAG_PLAN_MARGIN, 0);
+//
+//
+//
+// ShmapSetRectangleFlag(0.73, 0.0, 0.99, 1.45, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.08, 0.0, 1.33, 1.45, MAP_FLAG_PLAN_MARGIN, 0);
+//
+// ShmapSetRectangleFlag(0.94, 1.66, 1.14, 1.66, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(0.91, 1.63, 1.16, 1.63, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(0.88, 1.61, 1.19, 1.61, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(0.85, 1.59, 1.21, 1.59, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(0.83, 1.56, 1.23, 1.56, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(0.81, 1.53, 1.26, 1.53, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(0.79, 1.51, 1.29, 1.51, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(0.76, 1.49, 1.31, 1.49, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(0.73, 1.47, 1.33, 1.47, MAP_FLAG_PLAN_MARGIN, 0);
+//
+// ShmapSetRectangleFlag(1.55, 0.65, 1.60, 2.1, MAP_FLAG_WALL, 0);
+//
+// ShmapSetRectangleFlag(1.28, 0.65, 1.54, 2.1, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.63, 0.65, 1.89, 2.1, MAP_FLAG_PLAN_MARGIN, 0);
+//
+// ShmapSetRectangleFlag(1.28, 0.63, 1.89, 0.63, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.31, 0.61, 1.86, 0.61, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.33, 0.59, 1.83, 0.59, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.36, 0.56, 1.81, 0.56, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.39, 0.53, 1.79, 0.53, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.41, 0.51, 1.76, 0.51, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.43, 0.49, 1.73, 0.49, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.46, 0.46, 1.71, 0.46, MAP_FLAG_PLAN_MARGIN, 0);
+// ShmapSetRectangleFlag(1.49, 0.43, 1.69, 0.43, MAP_FLAG_PLAN_MARGIN, 0);
+
+
randomize_obstacles(true);
+ // start point
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));
+ startx_real = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
+ starty_real = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0);
+ } while (!ShmapIsFreePoint(startx_real, starty_real));
+
+
/* Main boucle */
- for(i=1; i<2; i++){
- if (i%100 == 0) {
- randomize_obstacles(false);
- }
+ for(i=1; i < NBR_OF_TESTS; i++){
+// if (i%100 == 0) {
+// randomize_obstacles(false);
+// }
+
+ // goal point
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));
+ goalx_real = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
+ goaly_real = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM-50)+25)/1000.0);
+ } while (!ShmapIsFreePoint(goalx_real, goaly_real));
-// startx=0.2;
-// starty=0,6;
-// goalx=1.8;
-// goaly=1.8;
-
- val = path_planner(startx, starty, goalx, goaly , &path, &angle, &angle);
+ // or start and goal point manualy
+// startx_real=0.4;
+// starty_real=0.6;
+// goalx_real=2.6;
+// goaly_real=1.2;
+
+ // for png picture in every step
+ bool valid;
+ ShmapPoint2Cell(startx_real, starty_real, &startx, &starty, &valid);
+ ShmapSetCellFlag(startx, starty, MAP_FLAG_START);
+
+ ShmapPoint2Cell(goalx_real, goaly_real, &goalx, &goaly, &valid);
+ ShmapSetCellFlag(goalx, goaly, MAP_FLAG_GOAL);
+
+ val = path_planner(startx_real, starty_real, goalx_real, goaly_real , &path, &angle, &angle);
- print_map("Found path:");
+ //print_map("Found path:");
- startx=goalx;starty=goaly;
+ startx_real=goalx_real;starty_real=goaly_real;
}
//Close File
return 1;
}
}
- // Free Memory
- //ShmapFree();
+
+#ifdef DBG_PNG
+ // final png picture, founded path
+ bool valid;
+ ShmapPoint2Cell(startx_real, starty_real, &startx, &starty, &valid);
+ ShmapSetCellFlag(startx, starty, MAP_FLAG_START);
+
+ ShmapPoint2Cell(goalx_real, goaly_real, &goalx, &goaly, &valid);
+ ShmapSetCellFlag(goalx, goaly, MAP_FLAG_GOAL);
+
+ char file_name[50];
+ sprintf(file_name,"testastar_%s.png",__TIME__);
+
+ if(!map_2_png(map, file_name)) {
+ printf("PATHPLAN: aAlgorithm : PNG file sucessfull created");
+ }
+ else {
+ printf("PATHPLAN: aAlgorithm : PNG file not created!");
+ }
+#endif
+ // Free Memory
+ freemem_aalgorithm();
+ ShmapFree();
+
return 0;
}
projects / eurobot / public.git / blobdiff