From: Matous Pokorny <matous.pokorny@me.com>
Date: Wed, 5 Jun 2013 15:23:00 +0000 (+0200)
Subject: Merge branch 'lidars' into ketchup
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/c59b1264a823e4772c486a219d84fc590fe7752d

Merge branch 'lidars' into ketchup
---

c59b1264a823e4772c486a219d84fc590fe7752d
diff --cc src/robofsm/map_handling.cc
index a19dfa91,08a00376..59ecb99a
--- a/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@@ -74,19 -194,73 +131,22 @@@ void update_map_lidar(const struct lida
  	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);
diff --cc src/robofsm/map_handling.h
index a7092ac1,e4d7ec32..e5aaa8d5
--- a/src/robofsm/map_handling.h
+++ b/src/robofsm/map_handling.h
@@@ -7,8 -8,9 +8,8 @@@
  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
  }