Merge branch 'lidars' into ketchup

diff --combined src/robofsm/map_handling.cc
index a19dfa910d12aca70212e4e69da23841326cca34,08a0037683a6131852cf9d46572e75dd85bb89f7..59ecb99a2cf0803359422d81e4198d0af9b47569
--- 1/src/robofsm/map_handling.cc
--- 2/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@@ -2,9 -2,6 +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"
@@@ -21,7 -18,7 +18,7 @@@
  
  void obstacle_detected_at(double x, double y, bool real_obstacle)
  {
 -      int i,j, xcell, ycell;
 +      int xcell, ycell;
        struct robot_pos_type est_pos;
        struct map *map = robot.map;
        double xx, yy;
@@@ -43,6 -40,69 +40,6 @@@
                /* The obstacle was detected here */
                map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
        }
 -
 -      /** Then all the cells arround obstacle cell are set as
 -       * #MAP_NEW_OBSTACLE. Cells of current robot position are not
 -       * set to avoid path planning deadlock. If there are a path
 -       * cell between them, the path will be recalculated. @see
 -       * #OBS_CSPACE. */
 -
 -      /* Mark "protected" area around the obstacle */
 -      robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
 -
 -      int obst_size_cell = (int)ceil(OBS_SIZE_M/MAP_CELL_SIZE_M);
 -      for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
 -              for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
 -                      if (!ShmapIsCellInMap(i, j)) continue;
 -                      ShmapCell2Point(i, j, &xx, &yy);
 -                      if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
 -                          (distance(xx, yy, x, y) < OBS_SIZE_M)) { // We expect cirtular area around obstacle
 -                              map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
 -                      }
 -              }
 -      }
 -
 -}
 -
 -void figure_detected_at(double x, double y, const bool state)
 -{
 -      int i,j, xcell, ycell;
 -      struct robot_pos_type est_pos;
 -      struct map *map = robot.map;
 -      double xx, yy;
 -      bool valid;
 -
 -      if (!map)
 -              return;
 -
 -      ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
 -      /* Ignore obstacles outside playground */
 -      if (!valid)
 -              return;
 -
 -      /* Ignore obstacles at marked places */
 -      if (map->cells[ycell][xcell].flags & MAP_FLAG_IGNORE_OBST)
 -              return;
 -
 -      if (state) {
 -              map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
 -
 -              /* Mark "protected" area around the obstacle */
 -              robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
 -
 -              int obst_size_cell = (int)ceil(0.2/MAP_CELL_SIZE_M);
 -              for (i=(xcell-obst_size_cell); i <= xcell+obst_size_cell; i++) {
 -                      for (j=(ycell- obst_size_cell); j <=ycell + obst_size_cell; j++) {
 -                              if (!ShmapIsCellInMap(i, j)) continue;
 -                              ShmapCell2Point(i, j, &xx, &yy);
 -                              if ((distance(xx, yy, est_pos.x, est_pos.y) > IGNORE_CLOSER_THAN_M) &&
 -                                  (distance(xx, yy, x, y) < OBS_SIZE_M)) { // We expect cirtular area around obstacle
 -                                  map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
 -                              }
 -                      }
 -              }
 -
 -      }
  }
  
  /**
@@@ -66,33 -126,161 +63,96 @@@ void obst_coord(struct robot_pos_type *
        *y = sy+v*sin(sa);
  }
  
- void update_map_hokuyo(struct lidar_scan_type *s)
+ void get_checkerboard(std::vector<Shape_detect::Point> &team)
+ {
+       Shape_detect::Point tmp, newPoint, start;
+       unsigned int i;
+ 
+       if (robot.team_color) {
+               start.x = 0.625;
+               start.y = 0.525;
+       } else {
+               start.x = 0.975;
+               start.y = 0.525;
+ 
+       }
+ 
+       tmp = start;
+ 
+       for (i = 1; i < 4; i++) {
+               for (int j = 0; j < 3; j++) {
+                       newPoint.y = tmp.y + 0.7 * j;
+                       newPoint.x = tmp.x;
+                       team.push_back(newPoint);
+               }
+               tmp.x = tmp.x + 0.7;
+       }
+ 
+       if (robot.team_color) {
+               tmp.x = start.x + 0.35;
+               tmp.y = start.y + 0.35;
+       } else {
+               tmp.x = start.x - 0.35;
+               tmp.y = start.y + 0.35;
+ 
+       }
+ 
+       for (i = 1; i < 4; i++) {
+               for (int j = 0; j < 2; j++) {
+                       newPoint.y = tmp.y + 0.7 * j;
+                       newPoint.x = tmp.x;
+                       team.push_back(newPoint);
+               }
+               tmp.x = tmp.x + 0.7;
+       }
+ 
+       if (robot.team_color) {
+               tmp.x = 1.675;
+               tmp.y = 0.175;
+       } else {
+               tmp.x = 1.325;
+               tmp.y = 0.175;
+ 
+       }
+ 
+       team.push_back(tmp);
+ }
+ 
+ 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_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;
  
 -//    static std::vector<Shape_detect::Point> reds;
 -//    static std::vector<Shape_detect::Point> blues;
 -
 -//    if (reds.size() < 16) {
 -//            get_checkerboard(blues);
 -//            get_checkerboard(reds);
 -//    }
 -
        robot_get_est_pos(&e.x, &e.y, &e.phi);
  
-       beam.x = HOKUYO_CENTER_OFFSET_M;
+       beam.x = l->center_offset_m;
        beam.y = 0;
  
        data = s->data;
  
 -//    Shape_detect shapeDet;
 -//    std::vector<Shape_detect::Arc> arcs;
 -//    std::vector<Shape_detect::Point> center_arcs;
 -// 
 -//    shapeDet.prepare(data);
 -//    shapeDet.arc_detect(arcs);
 -// 
 -//    Shape_detect::Point tmpPoint;
 -
 -//    double distance;
 -
 -//    if (arcs.size() > 0) {
 -//            for (i = 0; i < arcs.size(); i++) {
 -//                    x = arcs[i].center.x / 1000;
 -//                    y = arcs[i].center.y / 1000;
 -//
 -//                    tmpPoint.x = e.x + x * cos(e.phi) - y * sin(e.phi);
 -//                    tmpPoint.y = e.y + x * sin(e.phi) + y * cos(e.phi);
 -//
 -//                    center_arcs.push_back(tmpPoint);
 -//            }
 -//
 -//            for (i = 0; i < center_arcs.size(); i++) {
 -//                    if (robot.team_color) {
 -//                            for (unsigned int j = 0; j < blues.size(); j++) {
 -//                                    distance = point_distance(blues[j], center_arcs[i]);
 -//                                    if (distance < 0.05) {
 -//                                            figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
 -//                                            break;
 -//                                    }
 -//                            }
 -//                    } else {
 -//                            for (unsigned int j = 0; j < reds.size(); j++) {
 -//                                    distance = point_distance(reds[j], center_arcs[i]);
 -//                                    if (distance < 0.05) {
 -//                                            figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
 -//                                            break;
 -//                                    }
 -//                            }
 -//                    }
 -//            1 = screws up}
 -//    }
 -
        bool obstacle = true;
  
-       for (i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
-               beam.ang = HOKUYO_INDEX_TO_RAD(i);
-               if((beam.ang < (-HOKUYO_RANGE_ANGLE_LEFT/180.0*M_PI)) || ((beam.ang > (HOKUYO_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);
  
 -//                    tmpPoint.x = x;
 -//                    tmpPoint.y = y;
 -
 -//                    if (center_arcs.size() > 0) {
 -//                            for (unsigned int j = 0; j < center_arcs.size(); j++) {
 -//                                    if (point_distance(tmpPoint, center_arcs[j]) < 0.12) {
 -//                                            obstacle = false;
 -//                                            break;
 -//                                    }
 -//                            }
 -//                    }
 -
                        if (obstacle) {
                                obstacle_detected_at(x, y, true);
 -                              //obst_coord(&e, &beam, (data[i]/1000.0)+0.3, &x, &y);
 -                              //obstacle_detected_at(x, y, false);
                        }
                        obstacle = true;
                }
@@@ -107,18 -295,18 +167,18 @@@
   * @see map #MAP_NEW_OBSTACLE
   * @todo Do faster this process. Use a cell's list to update.
   */
 -static void forget_obstacles(map_cell_detobst_t val){
 +static void locate_bear(){
        int i,j;
        struct map *map = robot.map;
  
 -        for (j=0;j<MAP_HEIGHT;j++){
 -                for(i=0;i<MAP_WIDTH;i++){
 +        for (j = 0; j < MAP_HEIGHT; j++){
 +                for(i = 0; i < MAP_WIDTH; i++){
                          struct map_cell *cell = &map->cells[j][i];
 -                        cell->flags &= ~MAP_FLAG_DET_OBST;
 -                        if (val < cell->detected_obstacle) cell->detected_obstacle -= val;
 -                        else {
 -                                cell->detected_obstacle = 0;
 -                                cell->flags &= ~MAP_FLAG_DET_OBST;
 +
 +                        if (cell->flags == MAP_FLAG_DET_OBST) {
 +                                ShmapCell2Point(i, j, &robot.target_pos.x, &robot.target_pos.y);
 +                                fprintf(stderr, "bear detected at x:%f y:%f\n", robot.target_pos.x, robot.target_pos.y);
 +                                FSM_SIGNAL(MAIN, EV_TARGET_DETECTED, NULL);
                          }
                  }
          }
@@@ -136,11 -324,20 +196,11 @@@ static void gettimeofday_ts(struct time
  /**
   * A thread updating the map
   */
 -void * thread_obstacle_forgeting(void * arg)
 +void * thread_target_detecion(void * arg)
  {
 -      struct timespec ts;
 -      sem_t timer;
 -      int val = (long long)MAP_NEW_OBSTACLE/(OBS_FORGET_SEC*1000/OBS_FORGET_PERIOD);
 -      if (val == 0) val = 1;
 -
 -      gettimeofday_ts(&ts);
 -      
 -      sem_init(&timer, 0, 0);
 -      while (1) {
 -              __fsm_timespec_add_ms(&ts, NULL, OBS_FORGET_PERIOD);
 -              sem_timedwait(&timer, &ts);
 -
 -              forget_obstacles(val);
 +        while (1) {
 +                if (robot.target_localization_enab == true)
 +                        locate_bear();
 +                usleep(500);
        }
  }

diff --combined src/robofsm/map_handling.h
index a7092ac110a11ec5b5e1424b4e663e4e7a379ed3,e4d7ec321703483c41ec7b3f394f0e29adf07c1b..e5aaa8d5addae8457f238495736003be3add1526
--- 1/src/robofsm/map_handling.h
--- 2/src/robofsm/map_handling.h
+++ b/src/robofsm/map_handling.h
@@@ -2,13 -2,15 +2,14 @@@
  #define _MAP_HANDLING_H
  
  #include <robodim.h>
+ #include <lidar.h>
  
  #ifdef __cplusplus
  extern "C" {
  #endif
  
 -void * thread_obstacle_forgeting(void * arg);
 -/*void update_map(struct sharps_type *s);*/
 +void * thread_target_detecion(void * arg);
- 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 --combined src/robofsm/robot_orte.c
index bb9763a2e2e90b37a45fc0a2d5e292a7f0119d81,ef3e3c6095449625689bbb61f03ae21104ccc6d5..a840e2258c57e644a9b3fc28a79b3f1cd59220b0
--- 1/src/robofsm/robot_orte.c
--- 2/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@@ -28,6 -28,7 +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 +326,7 @@@ void rcv_hokuyo_scan_cb(const ORTERecvI
                        ROBOT_UNLOCK(hokuyo);
                        if(!robot.ignore_hokuyo)
                        {
-                               //update_map_hokuyo(instance);
+                               //update_map_lidar(&hokuyo_params, instance);
                        }
                        break;
                }
@@@ -350,13 -351,12 +351,12 @@@ void rcv_sick_scan_cb(const ORTERecvInf
                        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;
        }
@@@ -372,7 -372,10 +372,7 @@@ void rcv_camera_result_cb(const ORTERec
                case NEW_DATA: {
                          if (instance->data_valid && instance->data_valid != last_response) {
                                  ROBOT_LOCK(camera_result);
 -                                robot.target_valid = instance->target_valid;
 -                                robot.target_ang = instance->angle_deg;
                                  ROBOT_UNLOCK(camera_result);
 -                                FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
                          }
                          last_response = instance->data_valid;
                        break;