robofsm : map_handling

[eurobot/public.git] / src / robofsm / map_handling.cc

diff --git a/src/robofsm/map_handling.cc b/src/robofsm/map_handling.cc
index 78cf67f70dffd6caf978ce393e5e6f6bfa0ea41c..ec0430009028c6ea4c6483308444c09c666dfdbd 100644 (file)
--- a/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@ -12,21 +12,15 @@
  * Parameters of Obstacle detection
  *******************************************************************************/
 
-const double OBS_SIZE_M = 0.2;  /**< Expected size of detected obstacle  */
-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) */
-const double IGNORE_FURTHER_THAN_M = 0.5; /**< Ignore data from sharp if further than this */
-const int OBS_FORGET_PERIOD = 100;  /**< The period of thread_obstacle_forgeting [ms] */
-const int OBS_FORGET_SEC = 1;  /**< Time to completely forget detected obstacle. */
-const double OBS_OFFSET = 0.6;
-
-void obstacle_detected_at(double x, double y, bool real_obstacle) {
+MapHandling map_handle;
+
+void MapHandling::obstacle_detected_at(double x, double y, bool real_obstacle) {
        int i,j, xcell, ycell;
        struct robot_pos_type est_pos;
-       struct map *map = robot.map;
        double xx, yy;
        bool valid;
 
-       if (!map)
+       if (!sh_map)
                return;
 
        ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
@@ -35,12 +29,12 @@ void obstacle_detected_at(double x, double y, bool real_obstacle) {
                return;
 
        /* Ignore obstacles at marked places */
-       if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
+       if (sh_map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
                return;
 
        if (real_obstacle) {
                /* The obstacle was detected here */
-               map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
+               sh_map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
        }
 
        /** Then all the cells arround obstacle cell are set as
@@ -59,21 +53,20 @@ void obstacle_detected_at(double x, double y, bool real_obstacle) {
                        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
-                               map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
+                               sh_map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
                        }
                }
        }
 
 }
 
-void figure_detected_at(double x, double y, const bool state) {
+void MapHandling::figure_detected_at(double x, double y, const bool state) {
        int i,j, xcell, ycell;
        struct robot_pos_type est_pos;
-       struct map *map = robot.map;
        double xx, yy;
        bool valid;
 
-       if (!map)
+       if (!sh_map)
                return;
 
        ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
@@ -82,11 +75,11 @@ void figure_detected_at(double x, double y, const bool state) {
                return;
 
        /* Ignore obstacles at marked places */
-       if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
+       if (sh_map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
                return;
 
        if (state) {
-               map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
+               sh_map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
 
                /* Mark "protected" area around the obstacle */
                robot.get_est_pos(est_pos.x, est_pos.y, est_pos.phi);
@@ -98,7 +91,7 @@ void figure_detected_at(double x, double y, const bool state) {
                                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
-                                   map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
+                                   sh_map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
                                }
                        }
                }
@@ -116,7 +109,7 @@ void figure_detected_at(double x, double y, const bool state) {
  *
  * @return
  */
-void obst_coord(struct robot_pos_type *e, const sharp_pos  *s, double v, double &x, double &y) {
+void MapHandling::obst_coord(struct robot_pos_type *e, const sharp_pos  *s, double v, double &x, double &y) {
        double sx, sy, sa;
        sx = e->x + s->x*cos(e->phi) - s->y*sin(e->phi);
        sy = e->y + s->x*sin(e->phi) + s->y*cos(e->phi);
@@ -126,7 +119,7 @@ void obst_coord(struct robot_pos_type *e, const sharp_pos  *s, double v, double
        y = sy+v*sin(sa);
 }
 
-void get_checkerboard(std::vector<Shape_detect::Point> &team) {
+void MapHandling::get_checkerboard(std::vector<Shape_detect::Point> &team) {
        Shape_detect::Point tmp, newPoint, start;
        unsigned int i;
 
@@ -180,11 +173,7 @@ void get_checkerboard(std::vector<Shape_detect::Point> &team) {
        team.push_back(tmp);
 }
 
-inline float point_distance(Shape_detect::Point a, Shape_detect::Point b) {
-       return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
-}
-
-void update_map_hokuyo(struct hokuyo_scan_type *s) {
+void MapHandling::update_map_hokuyo(struct hokuyo_scan_type *s) {
        double x, y;
        struct robot_pos_type e;
        unsigned int i;
@@ -291,7 +280,7 @@ void update_map_hokuyo(struct hokuyo_scan_type *s) {
  */
 static void forget_obstacles(map_cell_detobst_t val) {
        int i,j;
-       struct map *map = robot.map;
+       struct map *map = map_handle.get_map();
 
         for (j=0;j<MAP_HEIGHT;j++){
                 for(i=0;i<MAP_WIDTH;i++){