Subversion Repositories shark

/*

 * 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 2004-05-24 18:03:39 giacomo Exp $

 File:        $File$

 Revision:    $Revision: 1.1.1.1 $

 Last update: $Date: 2004-05-24 18:03:39 $

 ------------

**/

/*

 * 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;

}