WebSVN - shark - Rev 1132 - /demos/branches/pj/auto/sensor.c

/*
* Project: S.Ha.R.K.
*
* Coordinators:
* Giorgio Buttazzo <giorgio@sssup.it>
* Paolo Gai <pj@gandalf.sssup.it>
*
* Authors :
* Paolo Gai <pj@gandalf.sssup.it>
* Massimiliano Giorgi <massy@gandalf.sssup.it>
* Luca Abeni <luca@gandalf.sssup.it>
* (see the web pages for full authors list)
*
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
*
* http://www.sssup.it
* http://retis.sssup.it
* http://shark.sssup.it
*/

/**
------------
CVS : $Id: sensor.c,v 1.1.1.1 2002-09-02 09:37:42 pj Exp $

File: $File$
Revision: $Revision: 1.1.1.1 $
Last update: $Date: 2002-09-02 09:37:42 $
------------
**/

/*
* Copyright (C) 2000 Marco Dallera and Marco Fiocca
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/

/*
* AUTO
*
* Another Unuseful Track simulatOr
*
* Authors: Marco Dallera
* Marco Fiocca
*
*/

/* ------------ */
/* Camera car */
/* ------------ */

#include "include/auto.h"
#include "include/const.h"
#include "include/utils.h"
#include <drivers/glib.h>

#define RADIUS 1
#define TOLL_FACTOR 4.0
#define MIN_DIFF 3
#define ROAD_COLOR 27
#define OFFROAD_COLOR 0
#define DEBUG_COLOR GREEN
#define OUT_OF_TRACK 230
#define STRIP_DEFAULT TEL_WIDTH+1

extern sem_t grx_mutex;
extern car_params cars[MAX_CAR_NUMBER];

void scanline(point_f *buffer, float x1, float y1, float x2, float y2, int factor);
void get_camera_car(BYTE *buffer, car_status status);
int find_track(BYTE *image, road_info *info);
int find_opps(BYTE *image, road_info *info);
void find_collision(BYTE *image, road_info *info, car_status status);

/* ----------------------------------------------------------------- */

TASK sensor(int index) {
car_status actual_car_status; // the actual car state
road_info actual_road_info; // the actual road informations
BYTE image[TEL_WIDTH*TEL_HEIGHT]; // buffer contenente la visione della telecamera ruotata

point sq1, sq2;
point im1, im2;

CAB road_status_cab = cars[index].road_status_cab;
CAB car_status_cab = cars[index].car_status_cab;

// Draws display squares
sq1.x = TRACK_X1 + TRACK_WIDTH + TEL_HEIGHT;
sq1.y = TEL_HEIGHT*index + 1 + index;
sq2.x = sq1.x + TEL_WIDTH + 1;
sq2.y = sq1.y + TEL_HEIGHT + 1;

// Sets viewer coords
im1.x = sq1.x + 1;
im1.y = sq1.y + 1;
im2.x = im1.x + TEL_WIDTH - 1;
im2.y = im1.y + TEL_HEIGHT - 1;

// Inizializza il cab della strada
actual_road_info.left_border = TEL_WIDTH/2;
actual_road_info.right_border = TEL_WIDTH/2;
actual_road_info.distance = TEL_HEIGHT;
actual_road_info.curve = 0.0;
actual_road_info.opps_number = 0;
actual_road_info.flag = ROAD_OK;
actual_road_info.collision = NO_COLL;
set_road_info(actual_road_info, road_status_cab);

task_endcycle();

while(1) {
/* reads car informations */
actual_car_status = get_car_status(car_status_cab);

/* implementazione riconoscimento strada (sti cazzi!!!) */
get_camera_car(image, actual_car_status);
find_track(image, &actual_road_info);
find_opps(image, &actual_road_info);
find_collision(image, &actual_road_info, actual_car_status);

sem_wait(&grx_mutex);
grx_rect(sq1.x, sq1.y, sq2.x, sq2.y, cars[index].color);
grx_putimage(im1.x, im1.y, im2.x, im2.y, image);
sem_post(&grx_mutex);

/* sends informations to control task */
set_road_info(actual_road_info, road_status_cab);
task_endcycle();
}

}

/* ----------------------------------------------------------------- */

void find_collision(BYTE *image, road_info *info, car_status status)
{
point_f lt,rt,lb,rb,ct;
/* the lines used to scan the quadrilate */
point_f height_line[CAR_HEIGHT];
int i;
BYTE pix;

info->collision = NO_COLL;

ct.x = status.pos.x + (float)(CAR_HEIGHT/2 + 1) * cos(degree_to_rad(status.orient));
ct.y = status.pos.y - (float)(CAR_WIDTH/2 + 1) * sin(degree_to_rad(status.orient));

lb.x = ct.x - (float)(CAR_WIDTH/2.0) * cos(degree_to_rad(90-status.orient));
rb.x = ct.x + (float)(CAR_WIDTH/2.0) * cos(degree_to_rad(90-status.orient));
lt.x = lb.x - (float)(CAR_HEIGHT) * cos(degree_to_rad(status.orient));
rt.x = rb.x - (float)(CAR_HEIGHT) * cos(degree_to_rad(status.orient));

lb.y = ct.y - (float)(CAR_WIDTH/2.0) * sin(degree_to_rad(90-status.orient));
rb.y = ct.y + (float)(CAR_WIDTH/2.0) * sin(degree_to_rad(90-status.orient));
lt.y = lb.y + (float)(CAR_HEIGHT) * sin(degree_to_rad(status.orient));
rt.y = rb.y + (float)(CAR_HEIGHT) * sin(degree_to_rad(status.orient));

sem_wait(&grx_mutex);
scanline(height_line,rb.x,rb.y,rt.x,rt.y,CAR_HEIGHT);
for (i=0; i < CAR_HEIGHT; i++) {
pix = grx_getpixel(round(height_line[i].x),round(height_line[i].y));
if (pix != OFFROAD_COLOR && pix != ROAD_COLOR)
info->collision = COLLISION_RIGHT;
}
scanline(height_line,lb.x,lb.y,lt.x,lt.y,CAR_HEIGHT);
for (i=0; i < CAR_HEIGHT; i++) {
pix = grx_getpixel(round(height_line[i].x),round(height_line[i].y));
if (pix != OFFROAD_COLOR && pix != ROAD_COLOR)
info->collision = COLLISION_LEFT;
}
sem_post(&grx_mutex);

}

/* ----------------------------------------------------------------- */

/* Puts in a buffer the image viewed from the camera according to the car status */
void get_camera_car(BYTE *buffer, car_status status) {
/*
* lt ------ rt
* | |
* | |
* lb --ct-- rb
*/
point_f lt,rt,lb,rb,ct;
/* the lines used to scan the quadrilate */
point_f width_line[TEL_WIDTH];
point_f height_line[TEL_HEIGHT];
int i,j;
int tmpx,tmpy,tx,ty;
float x,y;
BYTE *row;

ct.x = status.pos.x + (float)(CAR_WIDTH/2 + 1) * cos(degree_to_rad(status.orient));
ct.y = status.pos.y - (float)(CAR_WIDTH/2 + 1) * sin(degree_to_rad(status.orient));

/* for now the visual starts from the center of the car */
lb.x = ct.x - (float)(TEL_WIDTH/2.0) * cos(degree_to_rad(90-status.orient));
rb.x = ct.x + (float)(TEL_WIDTH/2.0) * cos(degree_to_rad(90-status.orient));
lt.x = lb.x + (float)(TEL_HEIGHT) * cos(degree_to_rad(status.orient));
rt.x = rb.x + (float)(TEL_HEIGHT) * cos(degree_to_rad(status.orient));

lb.y = ct.y - (float)(TEL_WIDTH/2.0) * sin(degree_to_rad(90-status.orient));
rb.y = ct.y + (float)(TEL_WIDTH/2.0) * sin(degree_to_rad(90-status.orient));
lt.y = lb.y - (float)(TEL_HEIGHT) * sin(degree_to_rad(status.orient));
rt.y = rb.y - (float)(TEL_HEIGHT) * sin(degree_to_rad(status.orient));

/* scan only 2 sense thanks to parallelism */
/* ^ <-(height)
* ! lt
* ! |
* ! |
* ! lb ------ rb )
* 0 -------------> <-(width
*/
/* the width_line coordinates are relative, so in the for cycle
they are summarized to absolute height_line values */
scanline(width_line,0,0,rb.x - lb.x,rb.y - lb.y,TEL_WIDTH);
scanline(height_line,lb.x,lb.y,lt.x,lt.y,TEL_HEIGHT);

/* First fill row TEL_HEIGHT-1 of image and so on until row 0 */
sem_wait(&grx_mutex);
for (i=0; i < TEL_HEIGHT; i++) {
x = height_line[i].x;
y = height_line[i].y;
row = &buffer[TEL_WIDTH * (TEL_HEIGHT-1-i)];
for (j=0; j < TEL_WIDTH; j++) {
tmpx = round(x + width_line[j].x);
tmpy = round(y + width_line[j].y);

// Test rispetto ai bordi
if (tmpx >= 0 && tmpx < SCREEN_WIDTH && tmpy >= 0 && tmpy < SCREEN_HEIGHT) {
tx = tmpx - TRACK_X1-1;
ty = tmpy - TRACK_Y1-1;
if (tx >= 0 && tx < TRACK_WIDTH && ty >= 0 && ty < TRACK_HEIGHT)
row[j] = grx_getpixel(tmpx,tmpy);
else
row[j] = OUT_OF_TRACK;
} else {
row[j] = OUT_OF_TRACK;
}
}
}
sem_post(&grx_mutex);
}

/* ----------------------------------------------------------------- */

void scanline(point_f *buffer, float x1, float y1, float x2, float y2, int factor) {
int i;
float x,y;
float xstep,ystep;

xstep = (x2 - x1) / factor;
ystep = (y2 - y1) / factor;

// Start scan at <x1, y1>
x = x1;
y = y1;

for (i = 0; i < factor; i++) {
buffer[i].x = x;
buffer[i].y = y;
x += xstep;
y += ystep;
}
}

/* ----------------------------------------------------------------- */

/*
* Approssima la curva quando non è più possibile farlo con la scansione
* orizzontale
* - image
* - index l'indice da cui cominciare la ricerca della curva
* - distance la distanza dalla curva
*/
float approx_curve(BYTE *image, int index, int distance, int d_center, road_strip *strips) {
BYTE *p,tmp;
int i,j,k,ctrl=0;
point ms_center,ct;

// Decide se la curva e' verso sinistra o verso destra
p = &image[(TEL_HEIGHT - index - 1) * TEL_WIDTH];

ms_center.x = strips[index-1].left;
for (i = 0; i < min(3,strips[index-1].left); i++) {
// La curva è a destra
tmp = *(p + round(ms_center.x) - i);
if (tmp != ROAD_COLOR) {
ms_center.x = strips[index-1].right;
break;
}
}

p += round(ms_center.x);
// Scandisce la colonna fino alla fine della strada
for (i = index; i < TEL_HEIGHT; i++, p -= TEL_WIDTH) {
if (*p != ROAD_COLOR) break;
}

// Imposta il nuovo max_scost empirico
ms_center.y = (i-1 + index) / 2;

// i-1 è l'indice dell'ultima riga bianca
p = &image[(TEL_HEIGHT - (i-1) - 1) * TEL_WIDTH + round(ms_center.x)];
// scandisce da destra a sinistra
if (round(ms_center.x) > 0) {
for (j = 0; j < (round(ms_center.x) - strips[index-1].left); j++, p--) {
if (*p != ROAD_COLOR) break;
// scandisce dall'alto in basso finchè non trova nero
for (k = i-1; k > index; k--) {
tmp = *(p + (i-1-k)*TEL_WIDTH);
if (tmp != ROAD_COLOR) {
ms_center.y = (k + index) / 2;
ctrl = 1;
break;
}
}
if (ctrl) break;
}
ct.x = ms_center.x - j/2;
ct.y = ms_center.y;
} else {
// scandisce da sinistra a destra
for (j = 0; j < (strips[index-1].right - round(ms_center.x)); j++, p++) {
if (*p != ROAD_COLOR) break;
// scandisce dall'alto in basso finchè non trova nero
for (k = i-1; k > index; k--) {
tmp = *(p + (i-1-k)*TEL_WIDTH);
if (tmp != ROAD_COLOR) {
ms_center.y = (k + index) / 2;
ctrl = 1;
break;
}
}
if (ctrl) break;
}
ct.x = ms_center.x + j/2;
ct.y = ms_center.y;
}

return rad_to_degree(atan2((double)(ct.y - distance),(double)(ct.x - d_center)));
}

/* ----------------------------------------------------------------- */

int find_distance(road_strip *strips, int max_scost, int rett_tol) {
int i, distance;
int center_car = TEL_WIDTH/2;

// La distanza non puo' essere maggiore del max_scost
distance = max_scost;

for (i=1; i < max_scost; i++) {
// Calcola la distanza dalla curva
if (abs(strips[i].center - center_car) > rett_tol) {
distance = i;
break;
}
}
return distance;
}

/* ----------------------------------------------------------------- */

/* Scandisce la linea row e inserisce nella strip index le informazioni.
* Ritorna 1 se trova entrambi i bordi, altrimenti 0
*/
int scan_strip(BYTE *row, int index, road_strip *strips) {
// Tiene conto della posizione dei bordi precedenti per trattare
// meglio il caso di curve molto strette, dove possono esistere
// 2 bordi destri o sinistri

// Per evitare problemi nei casi in cui siano possibili 2
// bordi destri o sinistri
int lfound = 0;
int rfound = 0;
int center_car = TEL_WIDTH/2;
int i;

// Parte dal secondo pixel e finisce al penultimo
for(i=1; i < (TEL_WIDTH-1); i++) {
// Se trova la strada controlla se si tratta di bordo
// destro o sinistro
if (row[i] == ROAD_COLOR) {
// bordo sinistro
if (row[i - 1] != ROAD_COLOR) {
// Se ho già trovato questo tipo di bordo cerco
// quello + plausibile ...
if (lfound) {
// Test sulla striscia precedente
if (index > 0) {
if (abs(i - strips[index-1].left) <
abs(strips[index].left - strips[index-1].left))
strips[index].left = i;
} else {
// Test sul centro della macchina
if (abs(i - center_car) < abs(strips[0].left - center_car))
strips[index].left = i;
}
lfound++;
} else {
// ... altrimenti lo inizializzo
strips[index].left = i;
lfound = 1;
}
// bordo destro
} else if ((row[i + 1] != ROAD_COLOR) && lfound) {
// Se ho già trovato questo tipo di bordo cerco
// quello + plausibile ...
if (rfound) {
// Test sulla striscia precedente
if (index > 0) {
if (abs(i - strips[index-1].right) <
abs(strips[index].right - strips[index-1].right))
strips[index].right = i;
} else {
// Test sul centro della macchina
if (abs(i - center_car) < abs(strips[0].right - center_car))
strips[index].right = i;
}
rfound++;
} else {
// ... altrimenti lo inizializzo
strips[index].right = i;
rfound = 1;
}
}
}
}

// Calcola il centro della strada se trova i bordi
if (lfound && rfound)
strips[index].center = (strips[index].left + strips[index].right) / 2;

return (lfound && rfound);
}

/* ----------------------------------------------------------------- */

void where_am_i(BYTE *row, road_strip strip, road_info *info) {
int i;
int lfound = 0;
int rfound = 0;
int car_center = TEL_WIDTH/2;

for(i=1; i < (TEL_WIDTH-1); i++) {
// Se trova la strada controlla se si tratta di bordo
// destro o sinistro
if (row[i] == ROAD_COLOR) {
// bordo sinistro
if (row[i - 1] != ROAD_COLOR) {
strip.left = i;
lfound = 1;
break;
} else if (row[i + 1] != ROAD_COLOR) {
strip.right = i;
rfound = 1;
break;
}
}
}

if (lfound && rfound) {
info->flag = ROAD_OK;
}
else if (lfound && !rfound) info->flag = LEFT_ONLY;
else if (!lfound && rfound) info->flag = RIGHT_ONLY;
else {
if (row[car_center] == OFFROAD_COLOR) info->flag = NO_ROAD;
else info->flag = ROAD_OK;
}
info->left_border = car_center - strip.left;
info->right_border = strip.right - car_center;
}

/* ----------------------------------------------------------------- */

/* Analizza l'immagine vista dalla telecamera inserendo le informazioni
* necessarie in info, ovvero la distanza dai bordi destro e sinistro,
* la distanza dalla prossima curva e il suo angolo di curvatura.
* Per il momento ritorna -1 se non riesce a trovare entrambi i bordi della
* strada; in futuro gestiremo anche questa situazione.
*/
int find_track(BYTE *image, road_info *info) {
// vettore contenente le informazioni su ogni striscia
road_strip strips[TEL_HEIGHT];
BYTE *row;
int car_center = TEL_WIDTH/2;
int max_scost=0;
int once_scost=0;
int j, rett_tol=0;

info->flag = ROAD_OK;

strips[0].left = STRIP_DEFAULT;
strips[0].right = STRIP_DEFAULT;
/* Tratta in modo personalizzato il primo tratto di strada per
inizializzare alcune variabili e per capire situazioni particolari */
row = &image[(TEL_HEIGHT - 1)*TEL_WIDTH];
// Se trova i bordi ...
if (scan_strip(row,0,strips)) {
// ... calcola i bordi della strada e ...
info->left_border = car_center - strips[0].left;
info->right_border = strips[0].right - car_center;
// ... inizializza la tolleranza per il rettilineo.
rett_tol = (strips[0].right - strips[0].left) / TOLL_FACTOR;
} else {
// Se non trova il bordo ...
// Mi sono perso, cercami la strada
where_am_i(row,strips[0],info);
return 1;
}

/* Ora controlla le altre strisce della strada */

for(j=1; j < TEL_HEIGHT ; j++) {
row = &image[(TEL_HEIGHT - j - 1)*TEL_WIDTH];
// Se trova i bordi ...
if (scan_strip(row, j, strips)) {
// ... aggiorna l'indice di max scostamento.
if (abs(strips[j].center - car_center) >
abs(strips[max_scost].center - car_center)
&& abs(strips[j].center - car_center) > rett_tol) {

max_scost = j;
once_scost=1; // ha trovato almeno un max scost
}
} else {
// Se max_scost non è mai stato settato, prendo il max
if (!once_scost) {
max_scost = j-1;
once_scost = 1;
}

// se la distanza e il max_scost sono troppo vicini approssima la curva
info->distance = find_distance(strips, max_scost, rett_tol);
if ((max_scost - info->distance) < MIN_DIFF) {
info->curve = approx_curve(image, j, info->distance,
strips[info->distance].center,strips) - 90.0;
return 1;
}
break;
}
}

// Se max_scost non è mai stato settato, prendo il max
if (!once_scost) max_scost = TEL_HEIGHT-1;
max_scost = min(max_scost,TEL_HEIGHT-1);

info->distance = find_distance(strips,max_scost,rett_tol);

// calcola l'angolo di curvatura solo se il conto e' attendibile
if ((info->distance < (TEL_HEIGHT*0.9)) &&
((max_scost - info->distance) >= MIN_DIFF))
info->curve = rad_to_degree(atan2((double)(max_scost - info->distance),
(double)(strips[max_scost].center - strips[info->distance].center)));
else {
// se i conti sono empirici, si guarda la posizione dei due centri
if (strips[max_scost].center > strips[info->distance].center)
info->curve = 88.0;
else if (strips[max_scost].center < strips[info->distance].center)
info->curve = 92.0;
else
info->curve = 90.0;
}
info->curve -= 90.0;
return 0;
}

/* ----------------------------------------------------------------- */

int find_opps(BYTE *image, road_info *info)
{
int i, j;
int l_limit, r_limit; // the opposite empiric limits
BYTE *row;
int car_center = TEL_WIDTH/2;
int opps_number = 0;

// by default it needs to scan the entire row
l_limit = TEL_WIDTH - 1;
r_limit = 0;

// Scans one row at a time
for(j=1; j<TEL_HEIGHT; j++) {
row = &image[(TEL_HEIGHT - j - 1) * TEL_WIDTH];

// if some of the previous row has been identified as a car...
if (opps_number > 0 &&
j < (info->opps_list[opps_number - 1].y + CAR_HEIGHT + 1)) {
// excludes CAR_HEIGHT lines from the bottom if the car
l_limit = (car_center + info->opps_list[opps_number - 1].x) - CAR_WIDTH;
r_limit = (car_center + info->opps_list[opps_number - 1].x) + CAR_WIDTH;
}
else {
// it needs to scan the entire row
l_limit = TEL_WIDTH - 1;
r_limit = 0;
}

// Scans the current row
for (i=0; i<TEL_WIDTH; i++) {
if( (i < l_limit || i > r_limit) &&
row[i] != ROAD_COLOR && row[i] != OFFROAD_COLOR &&
row[i] != DEBUG_COLOR && row[i] != OUT_OF_TRACK) {
// there is an opponent car!!
// stores its position
info->opps_list[opps_number].x = i - car_center; // left -, right +
info->opps_list[opps_number].y = j;

// increases number of opposites car in the sensor view range
opps_number++;
}
}
}

info->opps_number = opps_number;

return 0;
}

Subversion Repositories shark

(root)/demos/branches/pj/auto/sensor.c - Rev 1132