robofsm: Update coding style

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

diff --git a/src/robofsm/map_handling.cc b/src/robofsm/map_handling.cc
index ec0430009028c6ea4c6483308444c09c666dfdbd..d5a8380f9bb5e525409326f4e324950fa6b0fe55 100644 (file)
--- a/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@ -16,7 +16,7 @@ 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;
+       robot_pos_type est_pos;
        double xx, yy;
        bool valid;
 
@@ -47,8 +47,8 @@ void MapHandling::obstacle_detected_at(double x, double y, bool real_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++) {
+       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) &&
@@ -62,7 +62,7 @@ void MapHandling::obstacle_detected_at(double x, double y, bool real_obstacle) {
 
 void MapHandling::figure_detected_at(double x, double y, const bool state) {
        int i,j, xcell, ycell;
-       struct robot_pos_type est_pos;
+       robot_pos_type est_pos;
        double xx, yy;
        bool valid;
 
@@ -84,9 +84,9 @@ void MapHandling::figure_detected_at(double x, double y, const bool state) {
                /* 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++) {
+               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) &&
@@ -95,7 +95,6 @@ void MapHandling::figure_detected_at(double x, double y, const bool state) {
                                }
                        }
                }
-
        }
 }
 
@@ -109,17 +108,17 @@ void MapHandling::figure_detected_at(double x, double y, const bool state) {
  *
  * @return
  */
-void MapHandling::obst_coord(struct robot_pos_type *e, const sharp_pos  *s, double v, double &x, double &y) {
+void MapHandling::obst_coord(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);
+       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);
        sa = e->phi + s->ang;
 
-       x = sx+v*cos(sa);
-       y = sy+v*sin(sa);
+       x = sx + v * cos(sa);
+       y = sy + v * sin(sa);
 }
 
-void MapHandling::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;
 
@@ -171,23 +170,15 @@ void MapHandling::get_checkerboard(std::vector<Shape_detect::Point> &team) {
        }
 
        team.push_back(tmp);
-}
+}*/
 
-void MapHandling::update_map_hokuyo(struct hokuyo_scan_type *s) {
+void MapHandling::update_map_hokuyo(hokuyo_scan_type *s) {
        double x, y;
-       struct robot_pos_type e;
+       robot_pos_type e;
        unsigned int i;
-       struct sharp_pos beam;
+       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;
@@ -204,7 +195,15 @@ void MapHandling::update_map_hokuyo(struct hokuyo_scan_type *s) {
 
        Shape_detect::Point tmpPoint;
 
-/*     double distance;
+/*     static std::vector<Shape_detect::Point> reds;
+       static std::vector<Shape_detect::Point> blues;
+
+       if (reds.size() < 16) {
+               get_checkerboard(blues);
+               get_checkerboard(reds);
+       }
+
+       double distance;
 
        if (arcs.size() > 0) {
                for (i = 0; i < arcs.size(); i++) {
@@ -242,7 +241,7 @@ void MapHandling::update_map_hokuyo(struct hokuyo_scan_type *s) {
 
        for (i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
                beam.ang = HOKUYO_INDEX_TO_RAD(i);
-               if((beam.ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*M_PI))||((beam.ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*M_PI))))
+               if((beam.ang < (-HOKUYO_RANGE_ANGLE_LEFT / 180.0 * M_PI) ) || (beam.ang > (HOKUYO_RANGE_ANGLE_RIGHT / 180.0 * M_PI) ) )
                        continue;
 
                if(data[i] > 19 && data[i] < 2000) {
@@ -280,23 +279,23 @@ void MapHandling::update_map_hokuyo(struct hokuyo_scan_type *s) {
  */
 static void forget_obstacles(map_cell_detobst_t val) {
        int i,j;
-       struct map *map = map_handle.get_map();
-
-        for (j=0;j<MAP_HEIGHT;j++){
-                for(i=0;i<MAP_WIDTH;i++){
-                        struct map_cell *cell = &map->cells[j][i];
-                        cell->flags &= ~MAP_FLAG_DET_OBST;
-                        if (val < cell->detected_obstacle) cell->detected_obstacle -= val;
-                        else {
-                                cell->detected_obstacle = 0;
-                                cell->flags &= ~MAP_FLAG_DET_OBST;
-                        }
-                }
-        }
+       map *map = map_handle.get_map();
+
+       for (j=0;j<MAP_HEIGHT;j++) {
+               for(i=0;i<MAP_WIDTH;i++) {
+                       map_cell *cell = &map->cells[j][i];
+                       cell->flags &= ~MAP_FLAG_DET_OBST;
+                       if (val < cell->detected_obstacle) cell->detected_obstacle -= val;
+                       else {
+                                 cell->detected_obstacle = 0;
+                                 cell->flags &= ~MAP_FLAG_DET_OBST;
+                       }
+               }
+       }
 }
 
 static void gettimeofday_ts(struct timespec *ts) {
-       struct timeval tv;
+       timeval tv;
        gettimeofday(&tv, NULL);
        /* Convert from timeval to timespec */
        ts->tv_sec = tv.tv_sec;