src/robofsm: Update map handler uses liblidar

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index 5002e44384a51ae347356f1ab3be32b9358bb561..e0556a4ae4b6dcd802817fdba367f448bcab127e 100644 (file)
--- a/src/robofsm/Makefile.omk
+++ b/src/robofsm/Makefile.omk
@@ -27,7 +27,7 @@ actlib_SOURCES = actuators.c
 # Libraries linked to all programs in this Makefile
 lib_LOADLIBES = robot mcl robomath roboorte robottype pthread rt m     \
                orte pathplan sharp map fsm rbtree motion robodim       \
-               actlib ulut shape_detect
+               actlib ulut shape_detect lidar
 
 # Automatic generation of event definition files
 include-pass_HOOKS = roboevent.c roboevent.h

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);

diff --git a/src/robofsm/map_handling.h b/src/robofsm/map_handling.h
index 22fdd02a971c545b778b0d30acf6da927ce7805f..e4d7ec321703483c41ec7b3f394f0e29adf07c1b 100644 (file)
--- a/src/robofsm/map_handling.h
+++ b/src/robofsm/map_handling.h
@@ -2,6 +2,7 @@
 #define _MAP_HANDLING_H
 
 #include <robodim.h>
+#include <lidar.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -9,7 +10,7 @@ extern "C" {
 
 void * thread_obstacle_forgeting(void * arg);
 /*void update_map(struct sharps_type *s);*/
-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 --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 2f4ff88456a3257673ffb68794da0e63e079bc16..216b3ab17b5c7ab30436906b11a8954c6c4ef96c 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -28,6 +28,7 @@
 #include <can_msg_def.h>
 #include <actuators.h>
 #include <ul_log.h>
+#include <lidar_params.h>
 
 UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
 
@@ -325,7 +326,7 @@ void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
                        ROBOT_UNLOCK(hokuyo);
                        if(!robot.ignore_hokuyo)
                        {
-                               //update_map_hokuyo(instance);
+                               //update_map_lidar(&hokuyo_params, instance);
                        }
                        break;
                }
@@ -350,13 +351,12 @@ void rcv_sick_scan_cb(const ORTERecvInfo *info, void *vinstance,
                        ROBOT_UNLOCK(sick);
                        if(!robot.ignore_sick)
                        {
-                               update_map_hokuyo(instance);
+                               update_map_lidar(&sick_params, instance);
                        }
                        break;
                }
                case DEADLINE:
                        robot.status[COMPONENT_SICK] = STATUS_FAILED;
-                       //system("killall -9 hokuyo");
                        DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
                        break;
        }

diff --git a/src/robofsm/test/Makefile.omk b/src/robofsm/test/Makefile.omk
index e39b3e10cf5f0d77724fb33c4a822d325934307e..08ea67e16d5fe99e833b744e4f83961b2b2bae36 100644 (file)
--- a/src/robofsm/test/Makefile.omk
+++ b/src/robofsm/test/Makefile.omk
@@ -43,5 +43,5 @@ mcl-laser_SOURCES = mcl-laser.cc
 # Libraries linked to all programs in this Makefile
 lib_LOADLIBES = robot mcl robodim laser-nav robomath roboorte          \
                robottype pthread rt m orte pathplan sharp map fsm      \
-               rbtree motion actlib ulut shape_detect
+               rbtree motion actlib ulut shape_detect lidar