src/robofsm/map_handling.cc

   1 #include <robot.h>
   2 #include <robodim.h>
   3 #include <map.h>
   4 #include <robomath.h>
   5 #include <shape_detect.h>
   6
   7 #include "map_handling.h"
   8
   9 /*******************************************************************************
  10  * Parameters of Obstacle detection
  11  *******************************************************************************/
  12
  13 #define OBS_SIZE_M 0.2          /**< Expected size of detected obstacle  */
  14 #define IGNORE_CLOSER_THAN_M 0.2 /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */
  15 #define IGNORE_FURTHER_THAN_M 0.5 /**< Ignore data from sharp if further than this */
  16 #define OBS_FORGET_PERIOD       100             /**< The period of thread_obstacle_forgeting [ms] */
  17 #define OBS_FORGET_SEC 1                        /**< Time to completely forget detected obstacle. */
  18
  19 void obstacle_detected_at(double x, double y, bool real_obstacle)
  20 {
  21         int i,j, xcell, ycell;
  22         struct robot_pos_type est_pos;
  23         struct map *map = robot.map;
  24         double xx, yy;
  25         bool valid;
  26
  27         if (!map)
  28                 return;
  29
  30         ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
  31         /* Ignore obstacles outside playground */
  32         if (!valid)
  33                 return;
  34
  35         /* Ignore obstacles at marked places */
  36         if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
  37                 return;
  38
  39         if (real_obstacle) {
  40                 /* The obstacle was detected here */
  41                 map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
  42         }
  43
  44         /** Then all the cells arround obstacle cell are set as
  45          * #MAP_NEW_OBSTACLE. Cells of current robot position are not
  46          * set to avoid path planning deadlock. If there are a path
  47          * cell between them, the path will be recalculated. @see
  48          * #OBS_CSPACE. */
  49
  50         /* Mark "protected" area around the obstacle */
  51         robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
  52
  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
  67 void figure_detected_at(double x, double y, const bool state)
  68 {
  69         int i,j, xcell, ycell;
  70         struct robot_pos_type est_pos;
  71         struct map *map = robot.map;
  72         double xx, yy;
  73         bool valid;
  74
  75         if (!map)
  76                 return;
  77
  78         ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
  79         /* Ignore obstacles outside playground */
  80         if (!valid)
  81                 return;
  82
  83         /* Ignore obstacles at marked places */
  84         if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
  85                 return;
  86
  87         if (state) {
  88                 map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
  89
  90                 /* Mark "protected" area around the obstacle */
  91                 robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
  92
  93                 int obst_size_cell = (int)ceil(0.2/MAP_CELL_SIZE_M);
  94                 for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
  95                         for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
  96                                 if (!ShmapIsCellInMap(i, j)) continue;
  97                                 ShmapCell2Point(i, j, &xx, &yy);
  98                                 if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
  99                                     (distance(xx, yy, x, y) < OBS_SIZE_M)) { // We expect cirtular area around obstacle
 100                                     map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
 101                                 }
 102                         }
 103                 }
 104
 105         }
 106 }
 107
 108 /**
 109  * A thread running the trajectory recalc
 110  *
 111  * This (low-medium priority) thread updates the map with sensors information.
 112  * If it is necesary, it recalculate the path.
 113  *
 114  * @param arg
 115  *
 116  * @return
 117  */
 118 void obst_coord(struct robot_pos_type *e, const struct sharp_pos  *s, double v, double *x, double *y)
 119 {
 120         double sx, sy, sa;
 121         sx = e->x + s->x*cos(e->phi) - s->y*sin(e->phi);
 122         sy = e->y + s->x*sin(e->phi) + s->y*cos(e->phi);
 123         sa = e->phi + s->ang;
 124
 125         *x = sx+v*cos(sa);
 126         *y = sy+v*sin(sa);
 127 }
 128
 129 void get_checkerboard(std::vector<Shape_detect::Point> &team)
 130 {
 131         Shape_detect::Point tmp, newPoint, start;
 132         unsigned int i;
 133
 134         if (robot.team_color) {
 135                 start.x = 0.625;
 136                 start.y = 0.525;
 137         } else {
 138                 start.x = 0.975;
 139                 start.y = 0.525;
 140
 141         }
 142
 143         tmp = start;
 144
 145         for (i = 1; i < 4; i++) {
 146                 for (int j = 0; j < 3; j++) {
 147                         newPoint.y = tmp.y + 0.7 * j;
 148                         newPoint.x = tmp.x;
 149                         team.push_back(newPoint);
 150                 }
 151                 tmp.x = tmp.x + 0.7;
 152         }
 153
 154         if (robot.team_color) {
 155                 tmp.x = start.x + 0.35;
 156                 tmp.y = start.y + 0.35;
 157         } else {
 158                 tmp.x = start.x - 0.35;
 159                 tmp.y = start.y + 0.35;
 160
 161         }
 162
 163         for (i = 1; i < 4; i++) {
 164                 for (int j = 0; j < 2; j++) {
 165                         newPoint.y = tmp.y + 0.7 * j;
 166                         newPoint.x = tmp.x;
 167                         team.push_back(newPoint);
 168                 }
 169                 tmp.x = tmp.x + 0.7;
 170         }
 171
 172         if (robot.team_color) {
 173                 tmp.x = 1.675;
 174                 tmp.y = 0.175;
 175         } else {
 176                 tmp.x = 1.325;
 177                 tmp.y = 0.175;
 178
 179         }
 180
 181         team.push_back(tmp);
 182 }
 183
 184 inline float point_distance(Shape_detect::Point a, Shape_detect::Point b)
 185 {
 186         return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
 187 }
 188
 189 void update_map_lidar(const struct lidar_params *l, struct lidar_scan_type *s)
 190 {
 191         double x, y;
 192         struct robot_pos_type e;
 193         int i;
 194         struct sharp_pos beam;
 195         u_int16_t *data;
 196
 197 //      static std::vector<Shape_detect::Point> reds;
 198 //      static std::vector<Shape_detect::Point> blues;
 199
 200 //      if (reds.size() < 16) {
 201 //              get_checkerboard(blues);
 202 //              get_checkerboard(reds);
 203 //      }
 204
 205         robot_get_est_pos(&e.x, &e.y, &e.phi);
 206
 207         beam.x = l->center_offset_m;
 208         beam.y = 0;
 209
 210         data = s->data;
 211
 212 //      Shape_detect shapeDet;
 213 //      std::vector<Shape_detect::Arc> arcs;
 214 //      std::vector<Shape_detect::Point> center_arcs;
 215 //
 216 //      shapeDet.prepare(data);
 217 //      shapeDet.arc_detect(arcs);
 218 //
 219 //      Shape_detect::Point tmpPoint;
 220
 221 //      double distance;
 222
 223 //      if (arcs.size() > 0) {
 224 //              for (i = 0; i < arcs.size(); i++) {
 225 //                      x = arcs[i].center.x / 1000;
 226 //                      y = arcs[i].center.y / 1000;
 227 //
 228 //                      tmpPoint.x = e.x + x * cos(e.phi) - y * sin(e.phi);
 229 //                      tmpPoint.y = e.y + x * sin(e.phi) + y * cos(e.phi);
 230 //
 231 //                      center_arcs.push_back(tmpPoint);
 232 //              }
 233 //
 234 //              for (i = 0; i < center_arcs.size(); i++) {
 235 //                      if (robot.team_color) {
 236 //                              for (unsigned int j = 0; j < blues.size(); j++) {
 237 //                                      distance = point_distance(blues[j], center_arcs[i]);
 238 //                                      if (distance < 0.05) {
 239 //                                              figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
 240 //                                              break;
 241 //                                      }
 242 //                              }
 243 //                      } else {
 244 //                              for (unsigned int j = 0; j < reds.size(); j++) {
 245 //                                      distance = point_distance(reds[j], center_arcs[i]);
 246 //                                      if (distance < 0.05) {
 247 //                                              figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
 248 //                                              break;
 249 //                                      }
 250 //                              }
 251 //                      }
 252 //              1 = screws up}
 253 //      }
 254
 255         bool obstacle = true;
 256
 257         for (i = 0; i < l->data_lenght; i++) {
 258                 beam.ang = index2rad(*l, i);
 259
 260                 if ((beam.ang<(-l->range_angle_left/180.0*M_PI)) ||
 261                         ((beam.ang>(l->range_angle_right/180.0*M_PI)))) {
 262                         continue;
 263                 }
 264
 265                 if(data[i] > 19 && data[i] < 2000) {
 266                         obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
 267
 268 //                      tmpPoint.x = x;
 269 //                      tmpPoint.y = y;
 270
 271 //                      if (center_arcs.size() > 0) {
 272 //                              for (unsigned int j = 0; j < center_arcs.size(); j++) {
 273 //                                      if (point_distance(tmpPoint, center_arcs[j]) < 0.12) {
 274 //                                              obstacle = false;
 275 //                                              break;
 276 //                                      }
 277 //                              }
 278 //                      }
 279
 280                         if (obstacle) {
 281                                 obstacle_detected_at(x, y, true);
 282                                 //obst_coord(&e, &beam, (data[i]/1000.0)+0.3, &x, &y);
 283                                 //obstacle_detected_at(x, y, false);
 284                         }
 285                         obstacle = true;
 286                 }
 287         }
 288 }
 289
 290 /**
 291  * Decrease map.detected_obstacle by val with saturation on zero. It
 292  * also clears #MAP_FLAG_DET_OBST if value reaches zero.
 293  *
 294  * @param val Value to decrease obstacle life.
 295  * @see map #MAP_NEW_OBSTACLE
 296  * @todo Do faster this process. Use a cell's list to update.
 297  */
 298 static void forget_obstacles(map_cell_detobst_t val){
 299         int i,j;
 300         struct map *map = robot.map;
 301
 302         for (j=0;j<MAP_HEIGHT;j++){
 303                 for(i=0;i<MAP_WIDTH;i++){
 304                         struct map_cell *cell = &map->cells[j][i];
 305                         cell->flags &= ~MAP_FLAG_DET_OBST;
 306                         if (val < cell->detected_obstacle) cell->detected_obstacle -= val;
 307                         else {
 308                                 cell->detected_obstacle = 0;
 309                                 cell->flags &= ~MAP_FLAG_DET_OBST;
 310                         }
 311                 }
 312         }
 313 }
 314
 315 static void gettimeofday_ts(struct timespec *ts)
 316 {
 317         struct timeval tv;
 318         gettimeofday(&tv, NULL);
 319         /* Convert from timeval to timespec */
 320         ts->tv_sec = tv.tv_sec;
 321         ts->tv_nsec = tv.tv_usec * 1000;
 322 }
 323
 324 /**
 325  * A thread updating the map
 326  */
 327 void * thread_obstacle_forgeting(void * arg)
 328 {
 329         struct timespec ts;
 330         sem_t timer;
 331         int val = (long long)MAP_NEW_OBSTACLE/(OBS_FORGET_SEC*1000/OBS_FORGET_PERIOD);
 332         if (val == 0) val = 1;
 333
 334         gettimeofday_ts(&ts);
 335
 336         sem_init(&timer, 0, 0);
 337         while (1) {
 338                 __fsm_timespec_add_ms(&ts, NULL, OBS_FORGET_PERIOD);
 339                 sem_timedwait(&timer, &ts);
 340
 341                 forget_obstacles(val);
 342         }
 343 }