robofsm: Map handling - revert back to original idea.

Mark circular area around real obstacle with MAP_NEW_OBSTACLE flag.
Radius of the area is of the ROBOT_DIAGONAL_RADIUS_M size instead of OBS_SIZE_M.
Remove flag wall from map.
Increase hokuyo range to 4 meters.

diff --git a/src/robofsm/map_handling.c b/src/robofsm/map_handling.c
index 1718437742403093ffc2c5cf839661fe02ac62af..699c82bd5ff2d1a66329caef7a9ba9f9323b4095 100644 (file)
--- a/src/robofsm/map_handling.c
+++ b/src/robofsm/map_handling.c
@@ -8,7 +8,6 @@
  * Parameters of Obstacle detection
  *******************************************************************************/
 
-#define OBS_SIZE_M 0.05                /**< Expected size of detected obstacle  */
 #define IGNORE_CLOSER_THAN_M 0.2 /**< Do not mark any obstacle closer than this to center of the robot (avoid path planning deadlock) */
 #define IGNORE_FURTHER_THAN_M 0.5 /**< Ignore data from sharp if further than this */
 #define OBS_FORGET_PERIOD      200             /**< The period of thread_obstacle_forgeting [ms] */
@@ -37,7 +36,6 @@ void obstacle_detected_at(double x, double y, bool real_obstacle)
        if (real_obstacle) {
                /* The obstacle was detected here */
                map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
-                map->cells[ycell][xcell].flags |= MAP_FLAG_WALL;
                 map->cells[ycell][xcell].detected_obstacle |= MAP_NEW_OBSTACLE;
        }
 
@@ -47,22 +45,21 @@ void obstacle_detected_at(double x, double y, bool real_obstacle)
         * cell between them, the path will be recalculated. @see
         * #OBS_CSPACE. */
 
-#if 0
        /* Mark "protected" area around the obstacle */
        robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
 
-       int obst_size_cell = (int)ceil(OBS_SIZE_M/MAP_CELL_SIZE_M);
-       for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
-               for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
+       int safety_area = (int)ceil(ROBOT_DIAGONAL_RADIUS_M/MAP_CELL_SIZE_M);
+
+       for (i=(xcell-safety_area); i <= xcell+safety_area; i++) {
+               for (j=(ycell- safety_area); j <=ycell + safety_area; j++) {
                        if (!ShmapIsCellInMap(i, j)) continue;
+
                        ShmapCell2Point(i, j, &xx, &yy);
-                       if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
-                           (distance(xx, yy, x, y) < OBS_SIZE_M)) { /* We expect cirtular area around obstacle */
+                       if (distance(xx, yy, x, y) < ROBOT_DIAGONAL_RADIUS_M) {
                                map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
                        }
                }
        }
-#endif
 }
 /**
  * A thread running the trajectory recalc
@@ -107,9 +104,10 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
                if((beam.ang<(-70.0/180.0*M_PI))||((beam.ang>(70.0/180.0*M_PI))))
                        continue;
 
-               if(data[i] > 19 && data[i] < 2000) {
+               if(data[i] > 19 && data[i] < 4000) {
                        obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
                        obstacle_detected_at(x, y, true);
+                        obstacle_detected_at(x, y, false);
                }
        }
 }