From: Michal Vokac <vokac.m@gmail.com>
Date: Tue, 4 Jun 2013 18:32:12 +0000 (+0200)
Subject: Add unified ORTE topic type for LIDAR scan data
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/a0f420fc62d870bc5bd32f9a637852890c758942

Add unified ORTE topic type for LIDAR scan data
---

diff --git a/src/robofsm/map_handling.cc b/src/robofsm/map_handling.cc
index aaa7cdf2..68af7878 100644
--- a/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@ -189,7 +189,7 @@ 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 sick_scan_type *s)
+void update_map_hokuyo(struct lidar_scan_type *s)
 {
 	double x, y;
 	struct robot_pos_type e;
diff --git a/src/robofsm/map_handling.h b/src/robofsm/map_handling.h
index 9df92b77..22fdd02a 100644
--- a/src/robofsm/map_handling.h
+++ b/src/robofsm/map_handling.h
@@ -9,7 +9,7 @@ extern "C" {
 
 void * thread_obstacle_forgeting(void * arg);
 /*void update_map(struct sharps_type *s);*/
-void update_map_hokuyo(struct sick_scan_type *s);
+void update_map_hokuyo(struct lidar_scan_type *s);
 
 #ifdef __cplusplus
 }
diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index f7f69755..f47e23d8 100644
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -201,10 +201,10 @@ struct robot {
 	struct odo_data_type odo_data;		/* independent odometry */
 	struct corr_distances_type corr_distances;	/* ultrasound */
 
-	struct hokuyo_scan_type hokuyo;
+	struct lidar_scan_type hokuyo;
 	bool ignore_hokuyo;
         
-        struct sick_scan_type sick;
+        struct lidar_scan_type sick;
         bool ignore_sick;
 
         /* variables for target detection */
diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index e1b304ae..7fdb3317 100644
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -312,10 +312,35 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 	}
 }
 
+void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
+			void *recvCallBackParam)
+{
+	struct lidar_scan_type *instance = (struct lidar_scan_type *)vinstance;
+
+	switch (info->status) {
+		case NEW_DATA: {
+			ROBOT_LOCK(hokuyo);
+			robot.hokuyo = *instance;
+			robot.status[COMPONENT_HOKUYO] = STATUS_OK;
+			ROBOT_UNLOCK(hokuyo);
+			if(!robot.ignore_hokuyo)
+			{
+				//update_map_hokuyo(instance);
+			}
+			break;
+		}
+		case DEADLINE:
+			robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
+			//system("killall -9 hokuyo");
+			DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
+			break;
+	}
+}
+
 void rcv_sick_scan_cb(const ORTERecvInfo *info, void *vinstance,
 			void *recvCallBackParam)
 {
-	struct sick_scan_type *instance = (struct sick_scan_type *)vinstance;
+	struct lidar_scan_type *instance = (struct lidar_scan_type *)vinstance;
 
 	switch (info->status) {
 		case NEW_DATA: {
diff --git a/src/types/robottype.idl b/src/types/robottype.idl
index 8133653e..9db5bbcf 100644
--- a/src/types/robottype.idl
+++ b/src/types/robottype.idl
@@ -84,12 +84,10 @@ struct can_msg {
     octet id;
 };
 
-struct hokuyo_scan {
-    unsigned short data[681];
-};
-
-struct sick_scan {
-    unsigned short data[271];
+struct lidar_scan {
+	unsigned short lidar_type;
+	unsigned short data_lenght;
+	unsigned short data[681];
 };
 
 struct pos_short {
diff --git a/src/types/robottype.ortegen b/src/types/robottype.ortegen
index 55280903..773e50e0 100644
--- a/src/types/robottype.ortegen
+++ b/src/types/robottype.ortegen
@@ -11,8 +11,8 @@ type=can_msg		topic=can_msg
 type=robot_pos		topic=est_pos_odo
 type=robot_pos		topic=est_pos_indep_odo
 type=robot_pos		topic=est_pos_best
-type=hokuyo_scan	topic=hokuyo_scan	deadline=1
-type=sick_scan		topic=sick_scan		deadline=1
+type=lidar_scan         topic=hokuyo_scan       deadline=1
+type=lidar_scan         topic=sick_scan         deadline=1
 type=odo_data		topic=odo_data		deadline=0.3
 type=motion_irc		topic=motion_irc	deadline=0.3
 type=motion_speed	topic=motion_speed	deadline=0.3	pubdelay=0.1