src/robofsm/eb2008/map_handling.c

   1 #include <robot_eb2008.h>
   2 #include <robodim_eb2008.h>
   3 #include <map.h>
   4 #include <robomath.h>
   5
   6 /*******************************************************************************
   7  * Parameters of Obstacle detection
   8  *******************************************************************************/
   9
  10 #define OBS_SIZE_M 0.2          /**< Expected size of detected obstacle  */
  11 #define IGNORE_CLOSER_THAN_M 0.2 /**< Ignore detected which are closer than this to the robot (avoid path planning deadlock) */
  12 #define OBS_FORGET_PERIOD       500             /**< The period of thread_obstacle_forgeting [ms] */
  13 #define OBS_FORGET_SEC 5                        /**< Time to completely forget detected obstacle. */
  14 #define OBS_OFFSET      0.6
  15
  16 void obstacle_detected_at(double x, double y)
  17 {
  18         int i,j, xcell, ycell;
  19         struct est_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         if (!valid)
  29                 return;
  30
  31         /* The obstacle was detected here */
  32         map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
  33
  34         /** Then all the cells arround obstacle cell are set as
  35          * #MAP_NEW_OBSTACLE. Cells of current robot position are not
  36          * set to avoid path planning deadlock. If there are a path
  37          * cell between them, the path will be recalculated. @see
  38          * #OBS_CSPACE. */
  39
  40         /* Mark "protected" area around the obstacle */
  41         ROBOT_LOCK(est_pos);
  42         est_pos = robot.est_pos;
  43         ROBOT_UNLOCK(est_pos);
  44         int obst_size_cell = (int)ceil(OBS_SIZE_M/MAP_CELL_SIZE_M);
  45         for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
  46                 for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
  47                         if (!ShmapIsCellInMap(i, j)) continue;
  48                         ShmapCell2Point(i, j, &xx, &yy);
  49                         if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
  50                             (distance(xx, yy, x, y) < OBS_SIZE_M)) { /* We expect cirtular area around obstacle */
  51                                 map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
  52                         }
  53                 }
  54         }
  55 }
  56 /**
  57  * A thread running the trajectory recalc
  58  *
  59  * This (low-medium priority) thread updates the map with sensors information.
  60  * If it is necesary, it recalculate the path.
  61  *
  62  * @param arg
  63  *
  64  * @return
  65  */
  66
  67 void obst_coord(struct est_pos_type *e, const struct sharp_pos  *s, double v, double *x, double *y)
  68 {
  69         double sx, sy, sa;
  70         sx = e->x + s->x*cos(e->phi) - s->y*sin(e->phi);
  71         sy = e->y + s->x*sin(e->phi) + s->y*cos(e->phi);
  72         sa = e->phi + s->ang;
  73
  74         *x = sx+v*cos(sa);
  75         *y = sy+v*sin(sa);
  76 }
  77
  78 void update_map(struct sharps_type *s)
  79 {
  80         double val[SHARP_COUNT] = { s->left, s->right, s->front_left, s->front_right,
  81                                     s->back_left, s->back_right };
  82         int sharp80[] = {0,1,2,3};
  83         double x, y;
  84         //Pos p;
  85         struct est_pos_type e;
  86         int i, j;
  87
  88         ROBOT_LOCK(est_pos);
  89         e = robot.est_pos;
  90         ROBOT_UNLOCK(est_pos);
  91
  92         for (i=0; i<sizeof(sharp80)/sizeof(sharp80[0]); i++) {
  93                 j = sharp80[i];
  94                 if (val[j] < 0.7) {
  95                         obst_coord(&e, &sharp[j], val[j], &x, &y);
  96                         obstacle_detected_at(x, y);
  97
  98                         /* The second end of oponents robot */
  99                         obst_coord(&e, &sharp[j], val[j]+0.3, &x, &y);
 100                         obstacle_detected_at(x, y);
 101                 }
 102         }
 103 }
 104
 105 /**
 106  * Decrease map.detected_obstacle by val with saturation on zero. It
 107  * also clears #MAP_FLAG_DET_OBST if value reaches zero.
 108  *
 109  * @param val Value to decrease obstacle life.
 110  * @see map #MAP_NEW_OBSTACLE
 111  * @todo Do faster this process. Use a cell's list to update.
 112  */
 113 static void forget_obstacles(map_cell_detobst_t val){
 114         int i,j;
 115         struct map *map = robot.map;
 116
 117         for (j=0;j<MAP_HEIGHT;j++){
 118                 for(i=0;i<MAP_WIDTH;i++){
 119                         struct map_cell *cell = &map->cells[j][i];
 120                         if (val < cell->detected_obstacle) cell->detected_obstacle -= val;
 121                         else {
 122                                 cell->detected_obstacle = 0;
 123                                 cell->flags &= ~MAP_FLAG_DET_OBST;
 124                         }
 125                 }
 126         }
 127 }
 128
 129 static void gettimeofday_ts(struct timespec *ts)
 130 {
 131         struct timeval tv;
 132         gettimeofday(&tv, NULL);
 133         /* Convert from timeval to timespec */
 134         ts->tv_sec = tv.tv_sec;
 135         ts->tv_nsec = tv.tv_usec * 1000;
 136 }
 137
 138 /**
 139  * A thread updating the map
 140  */
 141 void * thread_obstacle_forgeting(void * arg)
 142 {
 143         struct timespec ts;
 144         sem_t timer;
 145         int val = (long long)MAP_NEW_OBSTACLE/(OBS_FORGET_SEC*1000/OBS_FORGET_PERIOD);
 146         if (val == 0) val = 1;
 147
 148         gettimeofday_ts(&ts);
 149
 150         sem_init(&timer, 0, 0);
 151         while (1) {
 152                 __fsm_timespec_add_ms(&ts, NULL, OBS_FORGET_PERIOD);
 153                 sem_timedwait(&timer, &ts);
 154
 155                 forget_obstacles(val);
 156         }
 157 }