robofsm: Remove unused code previously used for sick-day competition

diff --git a/src/robofsm/map_handling.cc b/src/robofsm/map_handling.cc
index 99f2994363f749a93e003ae4cda2cbdad152a68c..83e41f79da35bb02c54b5d1bad6632c10aa65797 100644 (file)
--- a/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@ -67,45 +67,6 @@ void obstacle_detected_at(double x, double y, bool real_obstacle)
 
 }
 
-void 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)
-               return;
-
-       ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
-       /* Ignore obstacles outside playground */
-       if (!valid)
-               return;
-
-       /* Ignore obstacles at marked places */
-       if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
-               return;
-
-       if (state) {
-               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);
-
-               int obst_size_cell = (int)ceil(0.2/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++) {
-                               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
-                                   map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
-                               }
-                       }
-               }
-
-       }
 }
 
 /**
@@ -129,66 +90,6 @@ void obst_coord(struct robot_pos_type *e, const struct sharp_pos  *s, double v,
        *y = sy+v*sin(sa);
 }
 
-void get_checkerboard(std::vector<Shape_detect::Point> &team)
-{
-       Shape_detect::Point tmp, newPoint, start;
-       unsigned int i;
-
-       if (robot.team_color) {
-               start.x = 0.625;
-               start.y = 0.525;
-       } else {
-               start.x = 0.975;
-               start.y = 0.525;
-
-       }
-
-       tmp = start;
-
-       for (i = 1; i < 4; i++) {
-               for (int j = 0; j < 3; j++) {
-                       newPoint.y = tmp.y + 0.7 * j;
-                       newPoint.x = tmp.x;
-                       team.push_back(newPoint);
-               }
-               tmp.x = tmp.x + 0.7;
-       }
-
-       if (robot.team_color) {
-               tmp.x = start.x + 0.35;
-               tmp.y = start.y + 0.35;
-       } else {
-               tmp.x = start.x - 0.35;
-               tmp.y = start.y + 0.35;
-
-       }
-
-       for (i = 1; i < 4; i++) {
-               for (int j = 0; j < 2; j++) {
-                       newPoint.y = tmp.y + 0.7 * j;
-                       newPoint.x = tmp.x;
-                       team.push_back(newPoint);
-               }
-               tmp.x = tmp.x + 0.7;
-       }
-
-       if (robot.team_color) {
-               tmp.x = 1.675;
-               tmp.y = 0.175;
-       } else {
-               tmp.x = 1.325;
-               tmp.y = 0.175;
-
-       }
-
-       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)
 {
        double x, y;
@@ -197,14 +98,6 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
        struct sharp_pos beam;
        u_int16_t *data;
 
-//     static std::vector<Shape_detect::Point> reds;
-//     static std::vector<Shape_detect::Point> blues;
-
-//     if (reds.size() < 16) {
-//             get_checkerboard(blues);
-//             get_checkerboard(reds);
-//     }
-
        robot_get_est_pos(&e.x, &e.y, &e.phi);
 
        beam.x = HOKUYO_CENTER_OFFSET_M;
@@ -212,49 +105,6 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
 
        data = s->data;
 
-//     Shape_detect shapeDet;
-//     std::vector<Shape_detect::Arc> arcs;
-//     std::vector<Shape_detect::Point> center_arcs;
-// 
-//     shapeDet.prepare(data);
-//     shapeDet.arc_detect(arcs);
-// 
-//     Shape_detect::Point tmpPoint;
-
-//     double distance;
-
-//     if (arcs.size() > 0) {
-//             for (i = 0; i < arcs.size(); i++) {
-//                     x = arcs[i].center.x / 1000;
-//                     y = arcs[i].center.y / 1000;
-//
-//                     tmpPoint.x = e.x + x * cos(e.phi) - y * sin(e.phi);
-//                     tmpPoint.y = e.y + x * sin(e.phi) + y * cos(e.phi);
-//
-//                     center_arcs.push_back(tmpPoint);
-//             }
-//
-//             for (i = 0; i < center_arcs.size(); i++) {
-//                     if (robot.team_color) {
-//                             for (unsigned int j = 0; j < blues.size(); j++) {
-//                                     distance = point_distance(blues[j], center_arcs[i]);
-//                                     if (distance < 0.05) {
-//                                             figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
-//                                             break;
-//                                     }
-//                             }
-//                     } else {
-//                             for (unsigned int j = 0; j < reds.size(); j++) {
-//                                     distance = point_distance(reds[j], center_arcs[i]);
-//                                     if (distance < 0.05) {
-//                                             figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
-//                                             break;
-//                                     }
-//                             }
-//                     }
-//             1 = screws up}
-//     }
-
        bool obstacle = true;
 
        for (i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
@@ -265,17 +115,6 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
                if(data[i] > 19 && data[i] < 2000) {
                        obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
 
-//                     tmpPoint.x = x;
-//                     tmpPoint.y = y;
-
-//                     if (center_arcs.size() > 0) {
-//                             for (unsigned int j = 0; j < center_arcs.size(); j++) {
-//                                     if (point_distance(tmpPoint, center_arcs[j]) < 0.12) {
-//                                             obstacle = false;
-//                                             break;
-//                                     }
-//                             }
-//                     }
 
                        if (obstacle) {
                                obstacle_detected_at(x, y, true);