src/robofsm/map_handling.c

   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 OBS_SIZE_M 0.2          /**< Expected size of detected obstacle  */
  12 #define IGNORE_CLOSER_THAN_M 0.2 /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */
  13 #define IGNORE_FURTHER_THAN_M 0.5 /**< Ignore data from sharp if further than this */
  14 #define OBS_FORGET_PERIOD       100             /**< The period of thread_obstacle_forgeting [ms] */
  15 #define OBS_FORGET_SEC 1                        /**< Time to completely forget detected obstacle. */
  16 #define OBS_OFFSET      0.6
  17
  18 void obstacle_detected_at(double x, double y, bool real_obstacle)
  19 {
  20         int i,j, xcell, ycell;
  21         struct robot_pos_type est_pos;
  22         struct map *map = robot.map;
  23         double xx, yy;
  24         bool valid;
  25
  26         if (!map)
  27                 return;
  28
  29         ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
  30         /* Ignore obstacles outside playground */
  31         if (!valid)
  32                 return;
  33
  34         /* Ignore obstacles at marked places */
  35         if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
  36                 return;
  37
  38         if (real_obstacle) {
  39                 /* The obstacle was detected here */
  40                 map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
  41         }
  42
  43         /** Then all the cells arround obstacle cell are set as
  44          * #MAP_NEW_OBSTACLE. Cells of current robot position are not
  45          * set to avoid path planning deadlock. If there are a path
  46          * cell between them, the path will be recalculated. @see
  47          * #OBS_CSPACE. */
  48
  49         /* Mark "protected" area around the obstacle */
  50         ROBOT_LOCK(est_pos_uzv);
  51         est_pos = robot.est_pos_uzv;
  52         ROBOT_UNLOCK(est_pos_uzv);
  53         int obst_size_cell = (int)ceil(OBS_SIZE_M/MAP_CELL_SIZE_M);
  54         for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
  55                 for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
  56                         if (!ShmapIsCellInMap(i, j)) continue;
  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) < OBS_SIZE_M)) { /* We expect cirtular area around obstacle */
  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 sharp_pos  *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->ang;
  82
  83         *x = sx+v*cos(sa);
  84         *y = sy+v*sin(sa);
  85 }
  86
  87 #if 0
  88 void update_map(struct sharps_type *s)
  89 {
  90         double val[SHARP_COUNT] = { s->left, s->right, s->front_left, s->front_right,
  91                                     s->back_left, s->back_right };
  92         int sharp80[] = {0,1,2,3};
  93         double x, y;
  94         //Pos p;
  95         struct robot_pos_type e;
  96         int i, j;
  97
  98         ROBOT_LOCK(est_pos_uzv);
  99         e = robot.est_pos_uzv;
 100         ROBOT_UNLOCK(est_pos_uzv);
 101
 102         for (i=0; i<sizeof(sharp80)/sizeof(sharp80[0]); i++) {
 103                 j = sharp80[i];
 104                 if (val[j] < IGNORE_FURTHER_THAN_M) {
 105                         obst_coord(&e, &sharp[j], val[j], &x, &y);
 106                         obstacle_detected_at(x, y, true);
 107
 108                         /* The second end of oponents robot */
 109                         obst_coord(&e, &sharp[j], val[j]+0.3, &x, &y);
 110                         obstacle_detected_at(x, y, false);
 111                 }
 112         }
 113 }
 114 #endif
 115
 116 void update_map_hokuyo(struct hokuyo_scan_type *s)
 117 {
 118         double x, y;
 119         //Pos p;
 120         struct robot_pos_type e;
 121         int i;
 122         struct sharp_pos beam;
 123         u_int16_t *data;
 124
 125         ROBOT_LOCK(est_pos_uzv);
 126         e = robot.est_pos_uzv;
 127         ROBOT_UNLOCK(est_pos_uzv);
 128
 129         beam.x = HOKUYO_CENTER_OFFSET_M;
 130         beam.y = 0;
 131
 132         data = s->data;
 133
 134         for (i = 0; i < HOKUYO_CLUSTER_CNT; i++) {
 135                 if((i<6)||((i>27)))
 136                         continue;
 137                 if(data[i] > 19) {
 138                         beam.ang = HOKUYO_CLUSTER_TO_RAD(i);
 139                         obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
 140                         obstacle_detected_at(x, y, true);
 141                         obst_coord(&e, &beam, (data[i]/1000.0)+0.3, &x, &y);
 142                         obstacle_detected_at(x, y, false);
 143                 }
 144
 145         }
 146 }
 147
 148 /**
 149  * Decrease map.detected_obstacle by val with saturation on zero. It
 150  * also clears #MAP_FLAG_DET_OBST if value reaches zero.
 151  *
 152  * @param val Value to decrease obstacle life.
 153  * @see map #MAP_NEW_OBSTACLE
 154  * @todo Do faster this process. Use a cell's list to update.
 155  */
 156 static void forget_obstacles(map_cell_detobst_t val){
 157         int i,j;
 158         struct map *map = robot.map;
 159
 160         for (j=0;j<MAP_HEIGHT;j++){
 161                 for(i=0;i<MAP_WIDTH;i++){
 162                         struct map_cell *cell = &map->cells[j][i];
 163                         cell->flags &= ~MAP_FLAG_DET_OBST;
 164                         if (val < cell->detected_obstacle) cell->detected_obstacle -= val;
 165                         else {
 166                                 cell->detected_obstacle = 0;
 167                                 cell->flags &= ~MAP_FLAG_DET_OBST;
 168                         }
 169                 }
 170         }
 171 }
 172
 173 static void gettimeofday_ts(struct timespec *ts)
 174 {
 175         struct timeval tv;
 176         gettimeofday(&tv, NULL);
 177         /* Convert from timeval to timespec */
 178         ts->tv_sec = tv.tv_sec;
 179         ts->tv_nsec = tv.tv_usec * 1000;
 180 }
 181
 182 /**
 183  * A thread updating the map
 184  */
 185 void * thread_obstacle_forgeting(void * arg)
 186 {
 187         struct timespec ts;
 188         sem_t timer;
 189         int val = (long long)MAP_NEW_OBSTACLE/(OBS_FORGET_SEC*1000/OBS_FORGET_PERIOD);
 190         if (val == 0) val = 1;
 191
 192         gettimeofday_ts(&ts);
 193
 194         sem_init(&timer, 0, 0);
 195         while (1) {
 196                 __fsm_timespec_add_ms(&ts, NULL, OBS_FORGET_PERIOD);
 197                 sem_timedwait(&timer, &ts);
 198
 199                 forget_obstacles(val);
 200         }
 201 }