robofsm: Map handling - update coding style

Change pointers to reference.

Fix some warnings.

diff --git a/src/robofsm/map_handling.cc b/src/robofsm/map_handling.cc
index baadd835705e2f48bbe1a0ff534f91f5df583b0c..e6133af0413f88721d6ca586f5f1efae45b83765 100644 (file)
--- a/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@ -12,12 +12,12 @@
  * Parameters of Obstacle detection
  *******************************************************************************/
 
-#define OBS_SIZE_M 0.2         /**< 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      100             /**< The period of thread_obstacle_forgeting [ms] */
-#define OBS_FORGET_SEC 1                       /**< Time to completely forget detected obstacle. */
-#define OBS_OFFSET     0.6
+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)
 {
@@ -118,15 +118,15 @@ void figure_detected_at(double x, double y, const bool state)
  *
  * @return
  */
-void obst_coord(struct robot_pos_type *e, const struct sharp_pos  *s, double v, double *x, double *y)
+void obst_coord(struct robot_pos_type *e, const struct 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);
        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 get_checkerboard(std::vector<Shape_detect::Point> &team)
@@ -221,39 +221,39 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
 
        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;
-//                                     }
-//                             }
-//                     }
-//             }
-//     }
+/*     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;
+                                       }
+                               }
+                       }
+               }
+       }*/
 
        bool obstacle = true;
 
@@ -263,7 +263,7 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
                        continue;
 
                if(data[i] > 19 && data[i] < 2000) {
-                       obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
+                       obst_coord(&e, &beam, data[i]/1000.0, x, y);
 
                        tmpPoint.x = x;
                        tmpPoint.y = y;
@@ -340,4 +340,5 @@ void * thread_obstacle_forgeting(void * arg)
 
                forget_obstacles(val);
        }
+       return NULL;
 }