From 5266ec02660106d2383bf3a38b3255577ca64a0a Mon Sep 17 00:00:00 2001 From: Michal Vokac Date: Tue, 4 Jun 2013 18:48:33 +0200 Subject: [PATCH] robofsm: Remove unused code previously used for sick-day competition --- src/robofsm/map_handling.cc | 161 ------------------------------------ 1 file changed, 161 deletions(-) diff --git a/src/robofsm/map_handling.cc b/src/robofsm/map_handling.cc index 99f29943..83e41f79 100644 --- 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 &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 reds; -// static std::vector 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 arcs; -// std::vector 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); -- 2.39.2