src/robofsm: Update map handler uses liblidar

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

diff --git a/src/robofsm/map_handling.cc b/src/robofsm/map_handling.cc
index 68af78780374b09df5dfec40f6621cc40e6cd13a..08a0037683a6131852cf9d46572e75dd85bb89f7 100644 (file)
--- a/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@ -2,9 +2,6 @@
 #include <robodim.h>
 #include <map.h>
 #include <robomath.h>
-#include <hokuyo.h>
-#include <sick.h>
-
 #include <shape_detect.h>
 
 #include "map_handling.h"
@@ -189,11 +186,11 @@ 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 lidar_scan_type *s)
+void update_map_lidar(const struct lidar_params *l, struct lidar_scan_type *s)
 {
        double x, y;
        struct robot_pos_type e;
-       unsigned int i;
+       int i;
        struct sharp_pos beam;
        u_int16_t *data;
 
@@ -207,7 +204,7 @@ void update_map_hokuyo(struct lidar_scan_type *s)
 
        robot_get_est_pos(&e.x, &e.y, &e.phi);
 
-       beam.x = SICK_CENTER_OFFSET_M;
+       beam.x = l->center_offset_m;
        beam.y = 0;
 
        data = s->data;
@@ -257,10 +254,13 @@ void update_map_hokuyo(struct lidar_scan_type *s)
 
        bool obstacle = true;
 
-       for (i = 0; i < SICK_ARRAY_SIZE; i++) {
-               beam.ang = SICK_INDEX_TO_RAD(i);
-               if((beam.ang<(-SICK_RANGE_ANGLE_LEFT/180.0*M_PI))||((beam.ang>(SICK_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);