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;
/* 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;
- }
- }
- }
-
- }
}
/**
*y = sy+v*sin(sa);
}
-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_hokuyo(struct lidar_scan_type *s)
{
double x, y;
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 = SICK_CENTER_OFFSET_M;
+ beam.x = HOKUYO_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 < SICK_ARRAY_SIZE; i++) {
- beam.ang = SICK_INDEX_TO_RAD(i);
- if((beam.ang<(-SICK_RANGE_ANGLE_LEFT/180.0*M_PI))||((beam.ang>(SICK_RANGE_ANGLE_RIGHT/180.0*M_PI))))
+ 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))))
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;
}
* @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);
}
}
}
/**
* 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);
}
}
projects / eurobot / public.git / blobdiff