7 /*******************************************************************************
8 * Parameters of Obstacle detection
9 *******************************************************************************/
11 #define IGNORE_CLOSER_THAN_M 2.0*MAP_CELL_SIZE_M /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */
12 #define IGNORE_FURTHER_THAN_M 0.5 /**< Ignore data from sharp if further than this */
13 #define OBS_FORGET_PERIOD 200 /**< The period of thread_obstacle_forgeting [ms] */
14 #define OBS_FORGET_SEC 5 /**< Time to completely forget detected obstacle. */
16 void obstacle_detected_at(double x, double y, bool real_obstacle)
18 int i,j, xcell, ycell;
19 struct robot_pos_type est_pos;
20 struct map *map = robot.map;
27 ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
28 /* Ignore obstacles outside playground */
32 /* Ignore obstacles at marked places */
33 if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
37 /* The obstacle was detected here */
38 map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
39 map->cells[ycell][xcell].detected_obstacle |= MAP_NEW_OBSTACLE;
42 /** Then all the cells arround obstacle cell are set as
43 * #MAP_NEW_OBSTACLE. Cells of current robot position are not
44 * set to avoid path planning deadlock. If there are a path
45 * cell between them, the path will be recalculated. @see
48 /* Mark "protected" area around the obstacle */
49 robot.get_est_pos(est_pos.x, est_pos.y, est_pos.phi);
51 int safety_area = (int)ceil(0.2/MAP_CELL_SIZE_M);
53 for (i=(xcell-safety_area); i <= xcell+safety_area; i++) {
54 for (j=(ycell- safety_area); j <=ycell + safety_area; j++) {
55 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) < ROBOT_DIAGONAL_RADIUS_M) {
60 map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
66 * A thread running the trajectory recalc
68 * This (low-medium priority) thread updates the map with sensors information.
69 * If it is necesary, it recalculate the path.
76 void obst_coord(struct robot_pos_type *e, const struct robot_pos_type *s, double v, double *x, double *y)
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);
87 void update_map_hokuyo(struct hokuyo_scan_type *s)
91 struct robot_pos_type e;
93 struct robot_pos_type beam;
96 robot.get_est_pos(e.x, e.y, e.phi);
98 beam.x = HOKUYO_CENTER_OFFSET_M;
103 for (i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
104 beam.phi = HOKUYO_INDEX_TO_RAD(i);
105 if((beam.phi<(-70.0/180.0*M_PI))||((beam.phi>(70.0/180.0*M_PI))))
108 if(data[i] > 19 && data[i] < 4000) {
109 obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
110 obstacle_detected_at(x, y, true);
111 obstacle_detected_at(x, y, false);
117 * Decrease map.detected_obstacle by val with saturation on zero. It
118 * also clears #MAP_FLAG_DET_OBST if value reaches zero.
120 * @param val Value to decrease obstacle life.
121 * @see map #MAP_NEW_OBSTACLE
122 * @todo Do faster this process. Use a cell's list to update.
124 static void forget_obstacles(map_cell_detobst_t val){
126 struct map *map = robot.map;
128 for (j=0;j<MAP_HEIGHT;j++){
129 for(i=0;i<MAP_WIDTH;i++){
130 struct map_cell *cell = &map->cells[j][i];
131 cell->flags &= ~MAP_FLAG_DET_OBST;
132 if (val < cell->detected_obstacle) cell->detected_obstacle -= val;
134 cell->detected_obstacle = 0;
135 cell->flags &= ~MAP_FLAG_DET_OBST;
141 static void gettimeofday_ts(struct timespec *ts)
144 gettimeofday(&tv, NULL);
145 /* Convert from timeval to timespec */
146 ts->tv_sec = tv.tv_sec;
147 ts->tv_nsec = tv.tv_usec * 1000;
151 * A thread updating the map
153 void * thread_obstacle_forgeting(void * arg)
157 int val = (long long)MAP_NEW_OBSTACLE/(OBS_FORGET_SEC*1000/OBS_FORGET_PERIOD);
158 if (val == 0) val = 1;
160 gettimeofday_ts(&ts);
162 sem_init(&timer, 0, 0);
164 __fsm_timespec_add_ms(&ts, NULL, OBS_FORGET_PERIOD);
165 sem_timedwait(&timer, &ts);
167 forget_obstacles(val);
171 void clear_flags_in_map(map_cell_flag_t flags)
174 struct map *map = robot.map;
176 for (y=0;y<MAP_HEIGHT;y++){
177 for(x=0;x<MAP_WIDTH;x++){
178 map->cells[y][x].flags &= ~flags;