From f928659d60bc3687819c8ae6d51ac3c3c989425f Mon Sep 17 00:00:00 2001
From: Matous Pokorny <matous.pokorny@me.com>
Date: Wed, 5 Jun 2013 17:16:55 +0200
Subject: [PATCH] src/robofsm: Update map handler uses liblidar

---
 src/robofsm/Makefile.omk      |  2 +-
 src/robofsm/map_handling.cc   | 18 +++++++++---------
 src/robofsm/map_handling.h    |  3 ++-
 src/robofsm/robot_orte.c      |  6 +++---
 src/robofsm/test/Makefile.omk |  2 +-
 5 files changed, 16 insertions(+), 15 deletions(-)

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index 5002e443..e0556a4a 100644
--- 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 68af7878..08a00376 100644
--- 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 22fdd02a..e4d7ec32 100644
--- 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 2f4ff884..216b3ab1 100644
--- 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 e39b3e10..08ea67e1 100644
--- 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
 
-- 
2.39.2