src/pathplan/map.c

   1 /**
   2  * @file        map.c
   3  * @brief       Useful functions related map
   4  * @author      Jose Maria Martin Laguna <jmmartin@etud.insa-toulouse.fr>
   5  *
   6  * This file contains functions to manage map.
   7  * The map should be accessed only by this library.
   8  *
   9  * @todo Lock the map! It necessary in multi-thread programs. If while
  10  * we are doing something in the map, somewhere the function
  11  * ShmapFree() is called, segfault occurs.
  12 */
  13
  14 #include "map.h"
  15 #include <sys/shm.h>
  16 #include <stdlib.h>
  17 #include <stdio.h>
  18 #include <sys/stat.h>
  19 #include <math.h>
  20
  21 struct map* map = NULL;
  22 int shmap_id;
  23 /** @addtogroup maplib */
  24 /**
  25  * @{
  26  *
  27  */
  28
  29
  30 /**
  31  * @name Shared Memory Map related functions
  32  * @{
  33  */
  34 /**
  35  * @brief Init Shared Map memory
  36  *
  37  * @param init_flag 1 to init the map memory and set all cells with
  38  *                  #MAP_FREE and #MAP_FLAG_NO_FLAG, 0 only init map
  39  * @return Pointer to the map. In case of error, exit() is called.
  40  *
  41  * This function allocates memory for shared map if this memory does
  42  * not exists.
  43  */
  44 struct map *ShmapInit(int init_flag){
  45         const int shmap_size = sizeof(struct map);
  46         if (map == NULL){
  47                 // Allocate memory to shared map
  48                 shmap_id = shmget (SHM_MAP_KEY , shmap_size, IPC_CREAT | S_IRUSR | S_IWUSR);    // Removed  flag IPC_EXCL
  49
  50                 if (shmap_id == -1) {
  51                         perror("shmget");
  52                         exit(1);
  53                 }
  54
  55                 /* Attach the shared memory segment.  */
  56                 map = (struct map *) shmat (shmap_id, 0, 0);
  57                 if ((int)map == -1) {
  58                         map = NULL;
  59                         perror("shmat");
  60                         exit(1);
  61                 }
  62
  63                 /* Initialize Map Memory */
  64                 if (init_flag) ShmapAllFreeSpace();
  65
  66                 //printf("Map initialized\n");
  67         }
  68         return map;
  69
  70 }
  71
  72 /**
  73  * @brief Free Shared Map memory
  74  */
  75 void ShmapFree(void){
  76         /* Deatch the shared memory segment.  */
  77         shmdt (map);
  78
  79         /* Deallocate the shared memory segment.  */
  80         shmctl (shmap_id, IPC_RMID, 0);
  81 }
  82
  83 /**
  84  * @brief Deatach Shared Map memory
  85  */
  86 void ShmapDt(void){
  87         /* Deatch the shared memory segment.  */
  88         shmdt (map);
  89
  90 }
  91 /**
  92  * @brief Check if Shared Map memory is init
  93  * @return      Pointer to the map or NULL if not initialized.
  94  */
  95 struct map * ShmapIsMapInit(void)
  96 {
  97         return map;
  98 }
  99 /** @} */
 100
 101
 102 /**
 103  * @name Map manage
 104  * @{
 105  */
 106 /**
 107  * @brief Initialize Map Memory with #MAP_FREE value and #MAP_FLAG_NO_FLAG flag.
 108  * @see map #MAP_FREE
 109 */
 110 void ShmapAllFreeSpace(void){
 111         int i,j;
 112         for (j=0;j<MAP_HEIGHT;j++){
 113                 for(i=0;i<MAP_WIDTH;i++){
 114                         map->cells[j][i].flags = 0;
 115                         map->cells[j][i].detected_obstacle = 0;
 116                 }
 117         }
 118 }
 119
 120 struct map_cell ShmapNoCell;
 121
 122 /**
 123  * @brief Put a value in a point
 124  * @param x_m   Coordonate X (in m) of a point
 125  * @param y_m   Coordonate Y (in m) of a point
 126  * @param value Value of the point
 127  * @return Pointer to the cell. If coordinates are outside of the map,
 128  *         pointer to ::ShmapNoCell is returned.
 129  */
 130 struct map_cell *ShmapCellAtPoint(double x_m, double y_m)
 131 {
 132         int i, j;
 133         bool valid;
 134         ShmapPoint2Cell(x_m, y_m, &i, &j, &valid);
 135         if (valid)
 136                 return &(map->cells[j][i]);
 137         else
 138                 return NULL;
 139 }
 140
 141 /**
 142  * @brief Give information about if a cell is free.
 143  * @param x     Coordonate of a cell
 144  * @param y     Coordonate of a cell
 145  * @return 1 if the cell is not an obstacle, -1 if the cell is not in
 146  * the map, 0 otherwise
 147  *
 148  * Following cells are consider as free cells: #MAP_START, #MAP_GOAL,
 149  * #MAP_PATH, #MAP_FREE.
 150  */
 151 int ShmapIsFreeCell(int x, int y)
 152 {
 153         struct map_cell *cell;
 154         if(map && ShmapIsCellInMap(x,y)) {
 155                 bool free;
 156                 cell = &(map->cells[y][x]);
 157                 free = (cell->detected_obstacle == 0) &&
 158                         (((cell->flags & (MAP_FLAG_WALL|MAP_FLAG_DET_OBST|MAP_FLAG_PLAN_MARGIN)) == 0) ||
 159                          ((cell->flags & MAP_FLAG_WALL) && (cell->flags & MAP_FLAG_INVALIDATE_WALL)));
 160                 return free;
 161         }
 162         else return -1;
 163 }
 164
 165 int ShmapIsFreePoint(double x_m, double y_m)
 166 {
 167         int i,j;
 168         bool valid;
 169         ShmapPoint2Cell(x_m, y_m, &i, &j, &valid);
 170         return ShmapIsFreeCell(i, j);
 171
 172 }
 173
 174
 175 /**
 176  * @brief Creates an obstacle in the map with a square shape
 177  * @param xs    Coordonate X (in m) of the central point
 178  * @param ys    Coordonate Y (in m) of the central point
 179  * @param r     Radius (in m)
 180  * @param cell  Type of obstacle.
 181  *
 182  */
 183 int ShmapSetCircleFlag(double xs, double ys, double r, map_cell_flag_t set_flags, map_cell_flag_t clear_flags)
 184 {
 185         int i,j, i1, i2, j1, j2;
 186
 187         // define a circumscribe square - just to shorten cyclic pass through the playground
 188         //ShmapPoint2Cell(xs-r, ys-r, &i1, &j1, &valid);
 189         //ShmapPoint2Cell(xs+r, ys+r, &i2, &j2, &valid);
 190
 191         i1 = 0;
 192         i2 = MAP_WIDTH;
 193         j1 = 0;
 194         j2 = MAP_HEIGHT;
 195
 196         double xp, yp;
 197         for(i=i1; i<=i2; i++) {
 198                 for(j=j1; j<=j2; j++) {
 199                         //DBG("Wall in (%d,%d)\n", i,j);
 200                         if (!ShmapIsCellInMap(i, j)) continue;
 201                         ShmapCell2Point(i, j, &xp, &yp); // get the central point of the cell
 202                         // determine if the CENTRAL point of the cell is inside the circle
 203                         if (! ( pow(xp-xs, 2)+pow(yp-ys, 2) < pow(r, 2) /*v kruhu*/)) continue;
 204                         map->cells[j][i].flags &= ~clear_flags;
 205                         map->cells[j][i].flags |= set_flags;
 206                 }
 207         }
 208         return 1;
 209 }
 210
 211 /**
 212  * @brief Creates an obstacle in the map
 213  * @param x1    Coordonate X (in m) of the first point
 214  * @param y1    Coordonate Y (in m) of the first point
 215  * @param x2    Coordonate X (in m) of the second point
 216  * @param y2    Coordonate Y (in m) of the second point
 217  * @param cell  Type of obstacle.
 218  *
 219  */
 220 int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_flag_t set_flags, map_cell_flag_t clear_flags)
 221 {
 222         int i,j, i1, i2, j1, j2;
 223         bool valid;
 224         ShmapPoint2Cell(x1, y1, &i1, &j1, &valid);
 225         ShmapPoint2Cell(x2, y2, &i2, &j2, &valid);
 226         if (i1 > i2) {
 227                 i = i2;
 228                 i2 = i1;
 229                 i1 = i;
 230         }
 231
 232         if (j1 > j2) {
 233                 j = j2;
 234                 j2 = j1;
 235                 j1 = j;
 236         }
 237
 238         //DBG("Drawing a rectangle between points (%d, %d) and (%d,%d)\n", init_i,init_j,limit_i,limit_j);
 239         for(i=i1; i<=i2; i++) {
 240                 for(j=j1; j<=j2; j++) {
 241                         //DBG("Wall in (%d,%d)\n", i,j);
 242                         if (!ShmapIsCellInMap(i, j)) continue;
 243                         map->cells[j][i].flags &= ~clear_flags;
 244                         map->cells[j][i].flags |= set_flags;
 245                 }
 246         }
 247         return 1;
 248 }
 249
 250 /** @} */
 251
 252
 253 /**
 254  * @name Coordonates translation
 255  * @{
 256  */
 257 /**
 258  * @brief Translates from real X coordonate (in meters) to grid cell coordonates
 259  * @param x     Coodonte of X
 260 */
 261
 262 void ShmapPoint2Cell(double x, double y, int *ix, int *iy, bool *valid){
 263         int xx, yy;
 264         xx = (int)floor(x/MAP_CELL_SIZE_M);
 265         yy = MAP_HEIGHT-1-(int)floor(y/MAP_CELL_SIZE_M);
 266
 267         if (valid != NULL) {
 268                 *valid = ( ( xx < MAP_WIDTH ) && (yy < MAP_HEIGHT) && ( xx >= 0 ) && ( yy >= 0) );
 269         }
 270         if (ix) *ix = xx;
 271         if (iy) *iy = yy;
 272 }
 273
 274 /**
 275  * @brief Translates from cell X coordonate to real coordonate.
 276  * @param x     Coodonte X of a cell.
 277  * @return X value in real coordonate, -1 if the coordonate is not in map
 278 */
 279 int ShmapCell2Point(int ix, int iy, double *x, double *y)
 280 {
 281         if ( (ix >= 0) && (ix <= MAP_WIDTH-1)) {
 282                 if (x) *x = (ix+0.5)*MAP_CELL_SIZE_M;
 283         }
 284         else return -1;
 285
 286         if ( (iy>=0) && (iy<=(MAP_HEIGHT-1))) {
 287                 if (y) *y =((MAP_HEIGHT-1-iy)+0.5)*MAP_CELL_SIZE_M;
 288         }
 289         else return -1;
 290         return 0;
 291 }
 292
 293 /**@}*/
 294
 295
 296 /**@}*/