Add unified ORTE topic type for LIDAR scan data

diff --git a/src/robofsm/map_handling.cc b/src/robofsm/map_handling.cc
index aaa7cdf25c2d479c8ea2a342222910d3d1e08dd7..68af78780374b09df5dfec40f6621cc40e6cd13a 100644 (file)
--- 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 9df92b77e85d46e76599db1fbbd87f9038f499ac..22fdd02a971c545b778b0d30acf6da927ce7805f 100644 (file)
--- 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 f7f697552a11d4ab9054d09d8409416dcdaa9b87..f47e23d8c8936caeba3f8efcd90ce458bf292533 100644 (file)
--- 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 e1b304ae772b16c810f2bd859e60e670de1e6654..7fdb3317b0a986844420738ad3cd61d8000ef91c 100644 (file)
--- 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 8133653ed335af52d4be7d10d4eb11d49b2a43ac..9db5bbcf195030612e1de1aa6f79d3974b94a4e0 100644 (file)
--- 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 552809031848ca0ee9855d73556bb3a665c4c838..773e50e07f6fe0c7d164a702fc24bab6d37adb71 100644 (file)
--- 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