robofsm: added orte subscriber for laser scanner data

[eurobot/public.git] / src / robofsm / eb2008 / robot_orte.c

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index e8ed13df9e0c5313644ced834e392b6569e15266..073da6331232709bd64fdee0291cf8b729b219a8 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -217,6 +217,25 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
+void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
+                       void *recvCallBackParam)
+{
+       struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
+
+       switch (info->status) {
+               case NEW_DATA: {
+                       ROBOT_LOCK(hokuyo);
+                       robot.hokuyo = *instance;
+                       ROBOT_UNLOCK(hokuyo);
+                       printf("hokyo: %d %d %d\n",instance->data1, instance->data2, instance->data3);
+                       update_map_hokuyo(instance);
+                       break;
+               }
+               case DEADLINE:
+                       DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
+                       break;
+       }
+}
 /* ---------------------------------------------------------------------- 
  * SUBSCRIBER CALLBACKS - EB2008
  * ---------------------------------------------------------------------- */
@@ -441,6 +460,7 @@ int robot_init_orte()
        generic_subscriber_pwr_voltage_create(&robot.gorte, rcv_pwr_voltage_cb, &robot.gorte);
        generic_subscriber_pwr_ctrl_create(&robot.gorte, rcv_pwr_ctrl_cb, &robot.gorte);
        generic_subscriber_robot_cmd_create(&robot.gorte, rcv_robot_cmd_cb, &robot.gorte);
+       generic_subscriber_hokuyo_scan_create(&robot.gorte, rcv_hokuyo_scan_cb, &robot.gorte);
 
        /* create eb2008 subscribers */
        eb2008_subscriber_servos_create(&robot.orte, rcv_servos_cb, &robot.orte);