src/pathplan/test/testbitmaps.c

   1 #define _ISOC99_SOURCE
   2 #include <stdio.h>
   3 #include <stdbool.h>
   4 #include <stdlib.h>
   5 #include "path_planner.h"
   6 #include <time.h>
   7 #include <sys/time.h>
   8 #include <math.h>
   9 #include <shist.h>
  10 #include "aalgorithm.h"
  11
  12 #define OBST_COUNT 4
  13 // #define POINT
  14
  15 void print_map(char *label)
  16 {
  17         int x, y;
  18         struct map *map = ShmapIsMapInit();
  19
  20         if (label)
  21                 printf("%s\n", label);
  22
  23         for (y=0; y<MAP_HEIGHT-1; y++) {
  24                 for (x=0; x<MAP_WIDTH; x++) {
  25                         char c;
  26                         if (ShmapIsFreeCell(x, y)) {
  27                                 c = '.';
  28                                 if (map->cells[y][x].flags & MAP_FLAG_PATH_SHAPE) {
  29                                         c = 's';
  30                                 }
  31                                 if (map->cells[y][x].flags & MAP_FLAG_PATH) {
  32                                         c = 'p';
  33                                 }
  34
  35                         } else
  36                                 c = '#';
  37                         putchar(c);
  38                 }
  39                 putchar('\n');
  40         }
  41 }
  42
  43
  44 int main(int argc, char *argv[])
  45 {
  46
  47
  48         bitmap_dim bitmap_par[4];
  49
  50         init_bitmap(bitmap_par, BITMAP_AB, BITMAP_AF, BITMAP_W2);
  51
  52         // Init Shared Map Memory
  53         ShmapInit(1);
  54
  55         // Create some obstacles
  56         ShmapSetRectangleFlag(0.0, 0.0, 0.1, 2.1, MAP_FLAG_WALL, 0);
  57         ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.1, MAP_FLAG_WALL, 0);
  58         ShmapSetRectangleFlag(0.0, 2.0, 3.0, 2.1, MAP_FLAG_WALL, 0);
  59         ShmapSetRectangleFlag(2.9, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0);
  60
  61 //      check_bot_position_straight(20,20,bitmap_par,0);
  62 //      check_bot_position_straight(40,20,bitmap_par,90);
  63 //      check_bot_position_straight(20,40,bitmap_par,180);
  64 //      check_bot_position_straight(40,40,bitmap_par,270);
  65 //
  66 //      ShmapSetCellFlag(20,20,MAP_FLAG_PATH);
  67 //      ShmapSetCellFlag(40,20,MAP_FLAG_PATH);
  68 //      ShmapSetCellFlag(20,40,MAP_FLAG_PATH);
  69 //      ShmapSetCellFlag(40,40,MAP_FLAG_PATH);
  70 //
  71         check_bot_position_across(20,15,bitmap_par,45);
  72         check_bot_position_across(20,30,bitmap_par,135);
  73         check_bot_position_across(20,45,bitmap_par,225);
  74         check_bot_position_across(20,60,bitmap_par,315);
  75
  76         ShmapSetCellFlag(20,15,MAP_FLAG_PATH);
  77         ShmapSetCellFlag(20,30,MAP_FLAG_PATH);
  78         ShmapSetCellFlag(20,45,MAP_FLAG_PATH);
  79         ShmapSetCellFlag(20,60,MAP_FLAG_PATH);
  80
  81         print_map("BITMAPS:");
  82
  83         // Free Memory
  84         //ShmapFree();
  85         return 0;
  86 }
  87
  88