Merge branch 'lidars' into ketchup

diff --cc src/robofsm/map_handling.cc
index a19dfa910d12aca70212e4e69da23841326cca34,08a0037683a6131852cf9d46572e75dd85bb89f7..59ecb99a2cf0803359422d81e4198d0af9b47569
--- 1/src/robofsm/map_handling.cc
--- 2/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@@ -74,19 -194,73 +131,22 @@@ void update_map_lidar(const struct lida
        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;
+       beam.x = l->center_offset_m;
        beam.y = 0;
  
        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++) {
-               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))))
+       for (i = 0; i < l->data_lenght; i++) {
+               beam.ang = index2rad(*l, i);
+ 
+               if ((beam.ang<(-l->range_angle_left/180.0*M_PI)) ||
+                       ((beam.ang>(l->range_angle_right/180.0*M_PI)))) {
                        continue;
+               }
  
                if(data[i] > 19 && data[i] < 2000) {
                        obst_coord(&e, &beam, data[i]/1000.0, &x, &y);

diff --cc src/robofsm/map_handling.h
index a7092ac110a11ec5b5e1424b4e663e4e7a379ed3,e4d7ec321703483c41ec7b3f394f0e29adf07c1b..e5aaa8d5addae8457f238495736003be3add1526
--- 1/src/robofsm/map_handling.h
--- 2/src/robofsm/map_handling.h
+++ b/src/robofsm/map_handling.h
@@@ -7,8 -8,9 +8,8 @@@
  extern "C" {
  #endif
  
 -void * thread_obstacle_forgeting(void * arg);
 -/*void update_map(struct sharps_type *s);*/
 +void * thread_target_detecion(void * arg);
- void update_map_hokuyo(struct lidar_scan_type *s);
+ void update_map_lidar(const struct lidar_params *l, struct lidar_scan_type *s);
  
  #ifdef __cplusplus
  }

diff --cc src/robofsm/robot_orte.c
Simple merge