robofsm: added function to update map with laser scanner data

diff --git a/src/robofsm/eb2008/map_handling.c b/src/robofsm/eb2008/map_handling.c
index f157c7b2569f8ff58869c78b98a0f661bb4d3b3e..38ffd63bbc10e1fc4afd22a01545da0a910d97bb 100644 (file)
--- a/src/robofsm/eb2008/map_handling.c
+++ b/src/robofsm/eb2008/map_handling.c
@@ -2,6 +2,7 @@
 #include <robodim_eb2008.h>
 #include <map.h>
 #include <robomath.h>
+#include <hokuyo.h>
 
 /*******************************************************************************
  * Parameters of Obstacle detection
@@ -110,6 +111,39 @@ void update_map(struct sharps_type *s)
        }
 }
 
+void update_map_hokuyo(struct hokuyo_scan_type *s)
+{
+       double x, y;
+       //Pos p;
+       struct est_pos_type e;
+       int i, j;
+       struct sharp_pos beam;
+       u_int16_t *data;
+
+       ROBOT_LOCK(est_pos);
+       e = robot.est_pos;
+       ROBOT_UNLOCK(est_pos);
+
+       beam.x = HOKUYO_CENTER_OFFSET_M;
+       beam.y = 0;
+
+       data = (u_int16_t*)&(s->data1);
+
+       for (i = 0; i < HOKUYO_CLUSTER_CNT; i++) {
+               if(data[i] > 19) {
+                       printf("mapa robot: %d %d %d\n", robot.hokuyo.data1, robot.hokuyo.data2,robot.hokuyo.data3);
+                       printf("mapa: %d %d\n", i, data[i]);
+                       beam.ang = HOKUYO_CLUSTER_TO_RAD(i);
+                       printf("mapa uhel: %f\n",beam.ang);
+                       obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
+                       obstacle_detected_at(x, y, true);
+                       obst_coord(&e, &beam, (data[i]/1000.0)+0.3, &x, &y);
+                       obstacle_detected_at(x, y, false);
+               }
+                       
+       }
+}
+
 /**
  * Decrease map.detected_obstacle by val with saturation on zero. It
  * also clears #MAP_FLAG_DET_OBST if value reaches zero.

diff --git a/src/robofsm/eb2008/map_handling.h b/src/robofsm/eb2008/map_handling.h
index d545a070f0e8a6b12a4e8cfd97068b94dd8e3f73..45ab168f2081abab51060fe9592bdd99d1500189 100644 (file)
--- a/src/robofsm/eb2008/map_handling.h
+++ b/src/robofsm/eb2008/map_handling.h
@@ -5,5 +5,6 @@
 
 void * thread_obstacle_forgeting(void * arg);
 void update_map(struct sharps_type *s);
+void update_map_hokuyo(struct hokuyo_scan_type *s);
 
 #endif