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                 cell = &(map->cells[y][x]);
 156                 return (cell->detected_obstacle == 0) && ((cell->flags & (MAP_FLAG_WALL|MAP_FLAG_DET_OBST)) == 0);
 157         }
 158         else return -1;
 159 }
 160
 161 int ShmapIsFreePoint(double x_m, double y_m)
 162 {
 163         int i,j;
 164         bool valid;
 165         ShmapPoint2Cell(x_m, y_m, &i, &j, &valid);
 166         return ShmapIsFreeCell(i, j);
 167
 168 }
 169
 170 /**
 171  * @brief Creates an obstacle in the map
 172  * @param x1    Coordonate X (in m) of the first point
 173  * @param y1    Coordonate Y (in m) of the first point
 174  * @param x2    Coordonate X (in m) of the second point
 175  * @param y2    Coordonate Y (in m) of the second point
 176  * @param cell  Type of obstacle.
 177  *
 178  */
 179 int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_flag_t set_flags, map_cell_flag_t clear_flags)
 180 {
 181         int i,j, i1, i2, j1, j2;
 182         bool valid;
 183         ShmapPoint2Cell(x1, y1, &i1, &j1, &valid);
 184         ShmapPoint2Cell(x2, y2, &i2, &j2, &valid);
 185         if (i1 > i2) {
 186                 i = i2;
 187                 i2 = i1;
 188                 i1 = i;
 189         }
 190
 191         if (j1 > j2) {
 192                 j = j2;
 193                 j2 = j1;
 194                 j1 = j;
 195         }
 196
 197         //DBG("Drawing a rectangle between points (%d, %d) and (%d,%d)\n", init_i,init_j,limit_i,limit_j);
 198         for(i=i1; i<=i2; i++) {
 199                 for(j=j1; j<=j2; j++) {
 200                         //DBG("Wall in (%d,%d)\n", i,j);
 201                         if (!ShmapIsCellInMap(i, j)) continue;
 202                         map->cells[j][i].flags &= ~clear_flags;
 203                         map->cells[j][i].flags |= set_flags;
 204                 }
 205         }
 206         return 1;
 207 }
 208
 209 /** @} */
 210
 211
 212 /**
 213  * @name Coordonates translation
 214  * @{
 215  */
 216 /**
 217  * @brief Translates from real X coordonate (in meters) to grid cell coordonates
 218  * @param x     Coodonte of X
 219 */
 220
 221 void ShmapPoint2Cell(double x, double y, int *ix, int *iy, bool *valid){
 222         int xx, yy;
 223         xx = (int)floor(x/MAP_CELL_SIZE_M);
 224         yy = MAP_HEIGHT-1-(int)floor(y/MAP_CELL_SIZE_M);
 225
 226         if (valid != NULL) {
 227                 *valid = ( ( xx < MAP_WIDTH ) && (yy < MAP_HEIGHT) && ( xx >= 0 ) && ( yy >= 0) );
 228         }
 229         if (ix) *ix = xx;
 230         if (iy) *iy = yy;
 231 }
 232
 233 /**
 234  * @brief Translates from cell X coordonate to real coordonate.
 235  * @param x     Coodonte X of a cell.
 236  * @return X value in real coordonate, -1 if the coordonate is not in map
 237 */
 238 int ShmapCell2Point(int ix, int iy, double *x, double *y)
 239 {
 240         if ( (ix >= 0) && (ix <= MAP_WIDTH-1)) {
 241                 if (x) *x = (ix+0.5)*MAP_CELL_SIZE_M;
 242         }
 243         else return -1;
 244
 245         if ( (iy>=0) && (iy<=(MAP_HEIGHT-1))) {
 246                 if (y) *y =((MAP_HEIGHT-1-iy)+0.5)*MAP_CELL_SIZE_M;
 247         }
 248         else return -1;
 249         return 0;
 250 }
 251
 252 /**@}*/
 253
 254
 255 /**@}*/