src/robofsm/map_handling.cc

   1 #include <robot.h>
   2 #include <robodim.h>
   3 #include <map.h>
   4 #include <robomath.h>
   5 #include <hokuyo.h>
   6
   7 /*******************************************************************************
   8  * Parameters of Obstacle detection
   9  *******************************************************************************/
  10
  11 #define IGNORE_CLOSER_THAN_M 2.0*MAP_CELL_SIZE_M /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */
  12 #define IGNORE_FURTHER_THAN_M 0.5 /**< Ignore data from sharp if further than this */
  13 #define OBS_FORGET_PERIOD       200             /**< The period of thread_obstacle_forgeting [ms] */
  14 #define OBS_FORGET_SEC 5                        /**< Time to completely forget detected obstacle. */
  15
  16 void obstacle_detected_at(double x, double y, bool real_obstacle)
  17 {
  18         int i,j, xcell, ycell;
  19         struct robot_pos_type est_pos;
  20         struct map *map = robot.map;
  21         double xx, yy;
  22         bool valid;
  23
  24         if (!map)
  25                 return;
  26
  27         ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
  28         /* Ignore obstacles outside playground */
  29         if (!valid)
  30                 return;
  31
  32         /* Ignore obstacles at marked places */
  33         if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
  34                 return;
  35
  36         if (real_obstacle) {
  37                 /* The obstacle was detected here */
  38                 map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
  39                 map->cells[ycell][xcell].detected_obstacle |= MAP_NEW_OBSTACLE;
  40         }
  41
  42         /** Then all the cells arround obstacle cell are set as
  43          * #MAP_NEW_OBSTACLE. Cells of current robot position are not
  44          * set to avoid path planning deadlock. If there are a path
  45          * cell between them, the path will be recalculated. @see
  46          * #OBS_CSPACE. */
  47
  48         /* Mark "protected" area around the obstacle */
  49         robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
  50
  51         int safety_area = (int)ceil(0.2/MAP_CELL_SIZE_M);
  52
  53         for (i=(xcell-safety_area); i <= xcell+safety_area; i++) {
  54                 for (j=(ycell- safety_area); j <=ycell + safety_area; j++) {
  55                         if (!ShmapIsCellInMap(i, j)) continue;
  56
  57                         ShmapCell2Point(i, j, &xx, &yy);
  58                         if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
  59                                 distance(xx, yy, x, y) < ROBOT_DIAGONAL_RADIUS_M) {
  60                                 map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
  61                         }
  62                 }
  63         }
  64 }
  65 /**
  66  * A thread running the trajectory recalc
  67  *
  68  * This (low-medium priority) thread updates the map with sensors information.
  69  * If it is necesary, it recalculate the path.
  70  *
  71  * @param arg
  72  *
  73  * @return
  74  */
  75
  76 void obst_coord(struct robot_pos_type *e, const struct robot_pos_type  *s, double v, double *x, double *y)
  77 {
  78         double sx, sy, sa;
  79         sx = e->x + s->x*cos(e->phi) - s->y*sin(e->phi);
  80         sy = e->y + s->x*sin(e->phi) + s->y*cos(e->phi);
  81         sa = e->phi + s->phi;
  82
  83         *x = sx+v*cos(sa);
  84         *y = sy+v*sin(sa);
  85 }
  86
  87 void update_map_hokuyo(struct hokuyo_scan_type *s)
  88 {
  89         double x, y;
  90         //Pos p;
  91         struct robot_pos_type e;
  92         int i;
  93         struct robot_pos_type beam;
  94         u_int16_t *data;
  95
  96         robot_get_est_pos(&e.x, &e.y, &e.phi);
  97
  98         beam.x = HOKUYO_CENTER_OFFSET_M;
  99         beam.y = 0;
 100
 101         data = s->data;
 102
 103         for (i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
 104                 beam.phi = HOKUYO_INDEX_TO_RAD(i);
 105                 if((beam.phi<(-70.0/180.0*M_PI))||((beam.phi>(70.0/180.0*M_PI))))
 106                         continue;
 107
 108                 if(data[i] > 19 && data[i] < 4000) {
 109                         obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
 110                         obstacle_detected_at(x, y, true);
 111                         obstacle_detected_at(x, y, false);
 112                 }
 113         }
 114 }
 115
 116 /**
 117  * Decrease map.detected_obstacle by val with saturation on zero. It
 118  * also clears #MAP_FLAG_DET_OBST if value reaches zero.
 119  *
 120  * @param val Value to decrease obstacle life.
 121  * @see map #MAP_NEW_OBSTACLE
 122  * @todo Do faster this process. Use a cell's list to update.
 123  */
 124 static void forget_obstacles(map_cell_detobst_t val){
 125         int i,j;
 126         struct map *map = robot.map;
 127
 128         for (j=0;j<MAP_HEIGHT;j++){
 129                 for(i=0;i<MAP_WIDTH;i++){
 130                         struct map_cell *cell = &map->cells[j][i];
 131                         cell->flags &= ~MAP_FLAG_DET_OBST;
 132                         if (val < cell->detected_obstacle) cell->detected_obstacle -= val;
 133                         else {
 134                                 cell->detected_obstacle = 0;
 135                                 cell->flags &= ~MAP_FLAG_DET_OBST;
 136                         }
 137                 }
 138         }
 139 }
 140
 141 static void gettimeofday_ts(struct timespec *ts)
 142 {
 143         struct timeval tv;
 144         gettimeofday(&tv, NULL);
 145         /* Convert from timeval to timespec */
 146         ts->tv_sec = tv.tv_sec;
 147         ts->tv_nsec = tv.tv_usec * 1000;
 148 }
 149
 150 /**
 151  * A thread updating the map
 152  */
 153 void * thread_obstacle_forgeting(void * arg)
 154 {
 155         struct timespec ts;
 156         sem_t timer;
 157         int val = (long long)MAP_NEW_OBSTACLE/(OBS_FORGET_SEC*1000/OBS_FORGET_PERIOD);
 158         if (val == 0) val = 1;
 159
 160         gettimeofday_ts(&ts);
 161
 162         sem_init(&timer, 0, 0);
 163         while (1) {
 164                 __fsm_timespec_add_ms(&ts, NULL, OBS_FORGET_PERIOD);
 165                 sem_timedwait(&timer, &ts);
 166
 167                 forget_obstacles(val);
 168         }
 169 }
 170
 171 void clear_flags_in_map(map_cell_flag_t flags)
 172 {
 173         int x, y;
 174         struct map *map = robot.map;
 175
 176         for (y=0;y<MAP_HEIGHT;y++){
 177                 for(x=0;x<MAP_WIDTH;x++){
 178                         map->cells[y][x].flags &= ~flags;
 179                 }
 180         }
 181 }