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 {
  24         int i,j, xcell, ycell;
  25         struct robot_pos_type est_pos;
  26         struct map *map = robot.map;
  27         double xx, yy;
  28         bool valid;
  29
  30         if (!map)
  31                 return;
  32
  33         ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
  34         /* Ignore obstacles outside playground */
  35         if (!valid)
  36                 return;
  37
  38         /* Ignore obstacles at marked places */
  39         if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
  40                 return;
  41
  42         if (real_obstacle) {
  43                 /* The obstacle was detected here */
  44                 map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
  45         }
  46
  47         /** Then all the cells arround obstacle cell are set as
  48          * #MAP_NEW_OBSTACLE. Cells of current robot position are not
  49          * set to avoid path planning deadlock. If there are a path
  50          * cell between them, the path will be recalculated. @see
  51          * #OBS_CSPACE. */
  52
  53         /* Mark "protected" area around the obstacle */
  54         robot.get_est_pos(est_pos.x, est_pos.y, est_pos.phi);
  55
  56         int obst_size_cell = (int)ceil(OBS_SIZE_M/MAP_CELL_SIZE_M);
  57         for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
  58                 for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
  59                         if (!ShmapIsCellInMap(i, j)) continue;
  60                         ShmapCell2Point(i, j, &xx, &yy);
  61                         if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
  62                             (distance(xx, yy, x, y) < OBS_SIZE_M)) { // We expect cirtular area around obstacle
  63                                 map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
  64                         }
  65                 }
  66         }
  67
  68 }
  69
  70 void figure_detected_at(double x, double y, const bool state)
  71 {
  72         int i,j, xcell, ycell;
  73         struct robot_pos_type est_pos;
  74         struct map *map = robot.map;
  75         double xx, yy;
  76         bool valid;
  77
  78         if (!map)
  79                 return;
  80
  81         ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
  82         /* Ignore obstacles outside playground */
  83         if (!valid)
  84                 return;
  85
  86         /* Ignore obstacles at marked places */
  87         if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
  88                 return;
  89
  90         if (state) {
  91                 map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
  92
  93                 /* Mark "protected" area around the obstacle */
  94                 robot.get_est_pos(est_pos.x, est_pos.y, est_pos.phi);
  95
  96                 int obst_size_cell = (int)ceil(0.2/MAP_CELL_SIZE_M);
  97                 for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
  98                         for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
  99                                 if (!ShmapIsCellInMap(i, j)) continue;
 100                                 ShmapCell2Point(i, j, &xx, &yy);
 101                                 if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
 102                                     (distance(xx, yy, x, y) < OBS_SIZE_M)) { // We expect cirtular area around obstacle
 103                                     map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
 104                                 }
 105                         }
 106                 }
 107
 108         }
 109 }
 110
 111 /**
 112  * A thread running the trajectory recalc
 113  *
 114  * This (low-medium priority) thread updates the map with sensors information.
 115  * If it is necesary, it recalculate the path.
 116  *
 117  * @param arg
 118  *
 119  * @return
 120  */
 121 void obst_coord(struct robot_pos_type *e, const struct sharp_pos  *s, double v, double &x, double &y)
 122 {
 123         double sx, sy, sa;
 124         sx = e->x + s->x*cos(e->phi) - s->y*sin(e->phi);
 125         sy = e->y + s->x*sin(e->phi) + s->y*cos(e->phi);
 126         sa = e->phi + s->ang;
 127
 128         x = sx+v*cos(sa);
 129         y = sy+v*sin(sa);
 130 }
 131
 132 void get_checkerboard(std::vector<Shape_detect::Point> &team)
 133 {
 134         Shape_detect::Point tmp, newPoint, start;
 135         unsigned int i;
 136
 137         if (robot.team_color) {
 138                 start.x = 0.625;
 139                 start.y = 0.525;
 140         } else {
 141                 start.x = 0.975;
 142                 start.y = 0.525;
 143
 144         }
 145
 146         tmp = start;
 147
 148         for (i = 1; i < 4; i++) {
 149                 for (int j = 0; j < 3; j++) {
 150                         newPoint.y = tmp.y + 0.7 * j;
 151                         newPoint.x = tmp.x;
 152                         team.push_back(newPoint);
 153                 }
 154                 tmp.x = tmp.x + 0.7;
 155         }
 156
 157         if (robot.team_color) {
 158                 tmp.x = start.x + 0.35;
 159                 tmp.y = start.y + 0.35;
 160         } else {
 161                 tmp.x = start.x - 0.35;
 162                 tmp.y = start.y + 0.35;
 163
 164         }
 165
 166         for (i = 1; i < 4; i++) {
 167                 for (int j = 0; j < 2; j++) {
 168                         newPoint.y = tmp.y + 0.7 * j;
 169                         newPoint.x = tmp.x;
 170                         team.push_back(newPoint);
 171                 }
 172                 tmp.x = tmp.x + 0.7;
 173         }
 174
 175         if (robot.team_color) {
 176                 tmp.x = 1.675;
 177                 tmp.y = 0.175;
 178         } else {
 179                 tmp.x = 1.325;
 180                 tmp.y = 0.175;
 181
 182         }
 183
 184         team.push_back(tmp);
 185 }
 186
 187 inline float point_distance(Shape_detect::Point a, Shape_detect::Point b)
 188 {
 189         return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
 190 }
 191
 192 void update_map_hokuyo(struct hokuyo_scan_type *s)
 193 {
 194         double x, y;
 195         struct robot_pos_type e;
 196         unsigned int i;
 197         struct sharp_pos beam;
 198         u_int16_t *data;
 199
 200         static std::vector<Shape_detect::Point> reds;
 201         static std::vector<Shape_detect::Point> blues;
 202
 203         if (reds.size() < 16) {
 204                 get_checkerboard(blues);
 205                 get_checkerboard(reds);
 206         }
 207
 208         robot.get_est_pos(e.x, e.y, e.phi);
 209
 210         beam.x = HOKUYO_CENTER_OFFSET_M;
 211         beam.y = 0;
 212
 213         data = s->data;
 214
 215         Shape_detect shapeDet;
 216         std::vector<Shape_detect::Arc> arcs;
 217         std::vector<Shape_detect::Point> center_arcs;
 218
 219         shapeDet.prepare(data);
 220         shapeDet.arc_detect(arcs);
 221
 222         Shape_detect::Point tmpPoint;
 223
 224 /*      double distance;
 225
 226         if (arcs.size() > 0) {
 227                 for (i = 0; i < arcs.size(); i++) {
 228                         x = arcs[i].center.x / 1000;
 229                         y = arcs[i].center.y / 1000;
 230
 231                         tmpPoint.x = e.x + x * cos(e.phi) - y * sin(e.phi);
 232                         tmpPoint.y = e.y + x * sin(e.phi) + y * cos(e.phi);
 233
 234                         center_arcs.push_back(tmpPoint);
 235                 }
 236
 237                 for (i = 0; i < center_arcs.size(); i++) {
 238                         if (robot.team_color) {
 239                                 for (unsigned int j = 0; j < blues.size(); j++) {
 240                                         distance = point_distance(blues[j], center_arcs[i]);
 241                                         if (distance < 0.05) {
 242                                                 figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
 243                                                 break;
 244                                         }
 245                                 }
 246                         } else {
 247                                 for (unsigned int j = 0; j < reds.size(); j++) {
 248                                         distance = point_distance(reds[j], center_arcs[i]);
 249                                         if (distance < 0.05) {
 250                                                 figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
 251                                                 break;
 252                                         }
 253                                 }
 254                         }
 255                 }
 256         }*/
 257
 258         bool obstacle = true;
 259
 260         for (i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
 261                 beam.ang = HOKUYO_INDEX_TO_RAD(i);
 262                 if((beam.ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*M_PI))||((beam.ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*M_PI))))
 263                         continue;
 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         return NULL;
 344 }