Subversion Repositories shark

Rev

Rev 1655 | Blame | Compare with Previous | Last modification | View Log | RSS feed

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