robofsm: Attempt to clear all old obstacles in map in area where nothing is detected.

This is lot of mathematical computation. Not good approach.

diff --git a/src/robofsm/map_handling.c b/src/robofsm/map_handling.c
index b8b50b91068a040cd357c63296cc43eb340767c9..1d7e6cdadf579259a659c6d91a40fa021062a654 100644 (file)
--- a/src/robofsm/map_handling.c
+++ b/src/robofsm/map_handling.c
@@ -8,7 +8,7 @@
  * Parameters of Obstacle detection
  *******************************************************************************/
 
-#define OBS_SIZE_M 0.2         /**< Expected size of detected obstacle  */
+#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,6 +37,8 @@ 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;
        }
 
        /** Then all the cells arround obstacle cell are set as
@@ -105,11 +107,29 @@ 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] < 4000) {
+               if(data[i] > 19 && data[i] < 2000) {
                        obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
                        obstacle_detected_at(x, y, true);
-               }
+               } /*else {
+                        //TODO delete all obsatcles previously seen in this range
+                        struct map *map = robot.map;
+
+                        double range;
+
+                        double cosinus = cos(e.phi + beam.ang);
+                        double sinus = sin(e.phi + beam.ang);
 
+                        for (range = 0.1; range < (data[i]/1000.0); range += 0.05) {
+                                //printf("pocitam, data[%i], range=%f\n", i, range);
+                                bool valid;
+
+                                beam.x = cosinus*range + e.x;
+                                beam.y = sinus*range + e.y;
+
+                                ShmapPoint2Cell(beam.x, beam.y, &x, &y, &valid);
+                                map->cells[(int)y][(int)x].flags |= MAP_FLAG_WALL;
+                        }
+                }*/
        }
 }