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