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 Set flag in the map cell
 213  * @param x     Coordonate X (int) of the cell
 214  * @param y     Coordonate Y (int) of the cell
 215  * @param set_flags     Map cell falgs
 216  *
 217  */
 218 int ShmapSetCellFlag(int x, int y, map_cell_flag_t set_flags)
 219 {
 220   if (!ShmapIsCellInMap(x, y)){
 221     return 0;
 222   }
 223
 224   map->cells[y][x].flags |= set_flags;
 225
 226   return 1;
 227 }
 228
 229 /** @} */
 230
 231 /**
 232  * @brief Creates an obstacle in the map
 233  * @param x1    Coordonate X (in m) of the first point
 234  * @param y1    Coordonate Y (in m) of the first point
 235  * @param x2    Coordonate X (in m) of the second point
 236  * @param y2    Coordonate Y (in m) of the second point
 237  * @param cell  Type of obstacle.
 238  *
 239  */
 240 int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_flag_t set_flags, map_cell_flag_t clear_flags)
 241 {
 242         int i,j, i1, i2, j1, j2;
 243         bool valid;
 244         ShmapPoint2Cell(x1, y1, &i1, &j1, &valid);
 245         ShmapPoint2Cell(x2, y2, &i2, &j2, &valid);
 246         if (i1 > i2) {
 247                 i = i2;
 248                 i2 = i1;
 249                 i1 = i;
 250         }
 251
 252         if (j1 > j2) {
 253                 j = j2;
 254                 j2 = j1;
 255                 j1 = j;
 256         }
 257
 258         //DBG("Drawing a rectangle between points (%d, %d) and (%d,%d)\n", init_i,init_j,limit_i,limit_j);
 259         for(i=i1; i<=i2; i++) {
 260                 for(j=j1; j<=j2; j++) {
 261                         //DBG("Wall in (%d,%d)\n", i,j);
 262                         if (!ShmapIsCellInMap(i, j)) continue;
 263                         map->cells[j][i].flags &= ~clear_flags;
 264                         map->cells[j][i].flags |= set_flags;
 265                 }
 266         }
 267         return 1;
 268 }
 269
 270 /** @} */
 271
 272
 273 /**
 274  * @name Coordonates translation
 275  * @{
 276  */
 277 /**
 278  * @brief Translates real point coordinates (in meters) to cell grid coordinates.
 279  * @param x     given x coordinate [meters]
 280  * @param y     given y coordinate [meters]
 281  * @param *ix   referenced int is set to grid index corresponding to x
 282  * @param *iy   referenced int is set to grid index corresponding to y
 283  * @param *valid referenced bool is set to true if and only if position given by (x,y) falls into playground
 284 */
 285
 286 void ShmapPoint2Cell(double x, double y, int *ix, int *iy, bool *valid){
 287         int xx, yy;
 288         xx = (int)floor(x/MAP_CELL_SIZE_M);
 289         yy = MAP_HEIGHT-1-(int)floor(y/MAP_CELL_SIZE_M);
 290
 291         if (valid != NULL) {
 292                 *valid = ( ( xx < MAP_WIDTH ) && (yy < MAP_HEIGHT) && ( xx >= 0 ) && ( yy >= 0) );
 293         }
 294         if (ix) *ix = xx;
 295         if (iy) *iy = yy;
 296 }
 297
 298 /**
 299  * @brief Translates from cell grid coordinates to real coordinates. Coordinates set to
 300    *x and *y are coordinates of the center of given cell.
 301  * @param ix    given x index of grid cell
 302  * @param iy    given y index of grid cell
 303  * @param *x    is set to real x coordinate corresponding to given ix
 304  * @param *y    is set to real y coordinate corresponding to given iy
 305  * @return 0 if and only if given grid indices fall into playground, otherwise returns -1
 306 */
 307 int ShmapCell2Point(int ix, int iy, double *x, double *y)
 308 {
 309         if ( (ix >= 0) && (ix <= MAP_WIDTH-1)) {
 310                 if (x) *x = (ix+0.5)*MAP_CELL_SIZE_M;
 311         }
 312         else return -1;
 313
 314         if ( (iy>=0) && (iy<=(MAP_HEIGHT-1))) {
 315                 if (y) *y =((MAP_HEIGHT-1-iy)+0.5)*MAP_CELL_SIZE_M;
 316         }
 317         else return -1;
 318         return 0;
 319 }
 320
 321 /**@}*/
 322
 323
 324 /**@}*/