Package components.gps.imagebased

Source Code of components.gps.imagebased.ImageBasedGPS

package components.gps.imagebased;

import java.awt.Color;
import java.awt.image.BufferedImage;

import utils.Semaphore;
import utils.defines.Defines;
import utils.images.ImageUtils;
import utils.images.ImageUtilsException;
import utils.images.RecognizedShape;

import components.gps.GPS;
import components.gps.GPSException;
import components.gps.RecognizedItems;
import components.imagecollector.ImageCollector;
import components.imagecollector.webcam.WebcamImageCollector;
import components.robot.RobotState;

import core.simulation.Simulation;



/**
* Esta clase implementa un <i>GPS</i> basado en im�genes. Es decir,
* un sistema de posicionamiento que es capaz de obtener la posici�n
* de los objetos en base a un procesamiento de la imagen del escenario
* en ese instante.<br>
* Puede ser usado tanto en simulaciones reales como en virtuales.
*/
public class ImageBasedGPS extends GPS
{
  // Atributos
  private  BufferedImage          image;
  private ImageUtils            imageUtils;
  private ImageCollector           ic;
  private RecognizedShape          robot;
  private RecognizedShape[]        newRobot;
  private Semaphore            gpsSemaphore;
  private double              xOld, yOld;

 
  // Constructor privado (por ser un singleton)
  public ImageBasedGPS()
  {
    super("WebCam GPS", "GPS a traves de una WebCam");
   
    imageUtils    = new ImageUtils();
    gpsSemaphore  = new Semaphore( 1 );
    robot      = null;
    xOld      = 0.0;
    yOld      = 0.0;
  }
 
 
  public synchronized BufferedImage getImage()
  {
    if ( image == null  ||  ic instanceof WebcamImageCollector )
      loadImage()// Para obtener una imagen "fresca"
    return image;
  }
 
 
  public synchronized void disposeImage()
  {
    image = null;
  }
 
 
  /**
   * Reconoce en la imagen corriente el robot representado por el
   * color (R,G,B) pasado por par�metro, y lo agrega a la lista de
   * robots reconocidos.
   * @param r        componente rojo.
   * @param g        componente verde.
   * @param b        componente azul.
   * @throws GPSException  si no se reconoce ning�n robot con dicho color,
   *             o si se reconoce m�s de uno.
   */
  public synchronized int recognizeRobot(int r, int g, int b) throws GPSException
  {
    loadImage()// Para obtener una imagen "fresca"
   
    if ( image == null )
      throw new GPSException("Null image.");
   
    try
    {
      if ( Simulation.getCurrent().getType() == Defines.REAL_SIMULATION )
        newRobot = imageUtils.recognizeShapes( r, g, b, 100, 999999, ImageUtils.HIGH_ACCURACY );
      else
        newRobot = imageUtils.recognizeShapes( r, g, b, 100, 999999, ImageUtils.NORMAL_ACCURACY );
     
      if ( newRobot == null  ||  newRobot.length == 0 )
        throw new GPSException("No se pudo reconocer el robot.");
     
      if (newRobot.length > 1)
        throw new GPSException"Se reconoci� m�s de un objeto con el mismo color. "  +
                    "Seleccione un color un�voco para el robot."      );

      gpsSemaphore.WAIT();
      newRobot[0].xOld  = xOld;
      newRobot[0].yOld  = yOld;
      robot        = newRobot[0];
      xOld        = robot.x;
      yOld        = robot.y;
      gpsSemaphore.SIGNAL();
     
      return 1;
    }
    catch (ImageUtilsException e)
    {
      throw new GPSException( e.getMessage() );
    }
   
  }


  /**
   * Reconoce en la imagen corriente los �conos coloreados representados por
   * el color (R,G,B) pasado por par�metro (y los agrega a la lista de
   * �conos coloreados reconocidos).
   * @param r        componente rojo.
   * @param g        componente verde.
   * @param b        componente azul.
   * @param x0      coordenada inicial en X de la seleccion
   * @param y0      coordenada inicial en Y de la seleccion
   * @param x1      coordenada final en X de la seleccion
   * @param y1      coordenada final en Y de la seleccion
   * @param name      nombre del �cono coloreado.
   * @throws GPSException  si no se reconoce ning�n �cono con dicho color.
   */
  public synchronized int recognizeColoredIcons(int r, int g, int b, int x0, int y0, int x1, int y1, String name) throws GPSException
  {
    loadImage()// Para obtener una imagen "fresca"
   
    if ( image == null )
      throw new GPSException("Null image.");

    try
    {
      RecognizedShape[] newIcons = null;
      //Si el �cono ya fue reconocido, se lo borra y se lo vuelve a reconocer
      if ( isColoredIconJustRecognized( r , g , b ) )
        removeRecognizedColoredIcon( r , g , b );
     
      if(Simulation.getCurrent().getType() == Defines.REAL_SIMULATION) {
        newIcons = recognizeIconsByShape(r, g, b, x0, y0, x1, y1);       
      } else {
        newIcons = recognizeIconsByColor(r, g, b);       
      }
     
      // Se agrega el nuevo �cono reconocido al final del arreglo
      RecognizedShape[] tempRecognizedColoredIcons = sceneItems.recognizedColoredIcons;
      sceneItems.recognizedColoredIcons = new RecognizedShape[sceneItems.recognizedColoredIcons.length + newIcons.length];
      int i,j;
      for (i=0; i<tempRecognizedColoredIcons.length; i++)
        sceneItems.recognizedColoredIcons[i] = tempRecognizedColoredIcons[i];
      for (j=0; j<newIcons.length; j++)
      {
        // A todos los iconos del mismo color se les pone el mismo nombre
        newIcons[j].shapeId = name;
        sceneItems.recognizedColoredIcons[j+i] = newIcons[j];
      }

      return newIcons.length;
    }
    catch (ImageUtilsException e)
    {
      throw new GPSException( e.getMessage() );
    }
  }


  private RecognizedShape[] recognizeIconsByShape(int r, int g, int b, int x0, int y0, int x1, int y1) { 
    RecognizedShape[] newIcons = new RecognizedShape[1];   
    RecognizedShape rs = buildShapeByBox(r, g, b, x0, y0, x1, y1);
    newIcons[0]=rs;   
    return newIcons;
  }


  private RecognizedShape buildShapeByBox(int r, int g, int b, int x0, int y0, int x1, int y1) {
    Color color = new Color(r,g,b);
   
    int xCentroid = x1-x0, yCentroid = y1-y0;
   
    int bx = Math.min(x0, x1);
    int by = Math.min(y0, y1);
   
    int area = Math.abs((x0-x1)*(y0-y1));
   
   
    int[] xp = new int[4];
    int[] yp = new int[4];
    xp[0] = x0; xp[1] = x0; xp[2] = x1; xp[3] = x1;
    yp[0] = y0; yp[1] = y1; yp[2] = y1; yp[3] = y0;

    RecognizedShape rs = new RecognizedShape(xCentroid,yCentroid,bx,by,area,color,xp,yp,4);
    return rs;
  }


  private RecognizedShape[] recognizeIconsByColor(int r, int g, int b) throws ImageUtilsException, GPSException {
    RecognizedShape[] newIcons;
    imageUtils.recognizeShapes( r, g, b, 1000, 999999, ImageUtils.HIGH_ACCURACY );
    newIcons = imageUtils.getRecognizedShapes();
    if ( newIcons == null  ||  newIcons.length == 0 )
      throw new GPSException("Not colored icons were recognized.");
    return newIcons;
  }


  /**
   * Carga la imagen SWT y realiza el procesamiento de reconocimiento
   * de paredes.
   * @param r    componentes <i>red</i> del color promedio de las paredes.
   * @param g    componentes <i>green</i> del color promedio de las paredes.
   * @param b    componentes <i>blue</i> del color promedio de las paredes.
   * @throws GPSException  si no hay una imagen establecida, o el procesamiento
   *             no pudo realizarse.
   */
  public synchronized RecognizedItems recognizeWalls(int r, int g, int b) throws GPSException
  {
    loadImage()// Para obtener una imagen "fresca"
   
    if ( image == null )
      throw new GPSException("Null image.");
   
    try
    {
      // Remuevo todas las paredes y las reconozco nuevamente
      removeAllRecognizedWalls();
      imageUtils.recognizeShapes(r, g, b, 2000, 999999, ImageUtils.HIGH_ACCURACY);
      sceneItems.recognizedWalls = imageUtils.getRecognizedShapes();
    }
    catch (ImageUtilsException e)
    {
      throw new GPSException( e.getMessage() );
    }

    return sceneItems;
  }

  /**
   * Si funciona el reconocimiento del riel, se hace, sino, se usa el cuadro
   * que llego por parametro
   * @param r    componentes <i>red</i> del color promedio de las paredes.
   * @param g    componentes <i>green</i> del color promedio de las paredes.
   * @param b    componentes <i>blue</i> del color promedio de las paredes.
   * @throws GPSException  si no hay una imagen establecida, o el procesamiento
   *             no pudo realizarse.
   */
  public synchronized RecognizedItems recognizeWalls(int x0, int y0, int x1, int y1, int r, int g, int b) throws GPSException
  {
    loadImage()// Para obtener una imagen "fresca"
   
    if ( image == null )
      throw new GPSException("Null image.");
   
    try
    {
      // Remuevo todas las paredes y las reconozco nuevamente
      removeAllRecognizedWalls();
      imageUtils.recognizeShapes(r, g, b, 2000, 999999, ImageUtils.HIGH_ACCURACY);
      sceneItems.recognizedWalls = imageUtils.getRecognizedShapes();
    }
    catch (ImageUtilsException e)
    {
      sceneItems.recognizedWalls = new RecognizedShape[] {buildShapeByBox(r, g, b, x0, y0, x1, y1)};
    }

    return sceneItems;
  }
 
  /**
   * Elimina el robot reconocido identificado por el nombre pasado por
   * par�metro.
   * @param name  nombre (un�voco) del robot a eliminar.
   */ 
  public void removeRecognizedRobot(String name)
  {
    robot = null;
  }


  /**
   * Verifica si el �cono con color (r,g,b) ya fue reconocido.
   * @param name  color (r,g,b) del �cono.
   * @return    <code>true</code> si el �cono ya fue reconocido, o
   *         <code>false</code> en caso contrario.
   */
  private boolean isColoredIconJustRecognized(int r, int g, int b)
  {
    for (int i=0; i<sceneItems.recognizedColoredIcons.length; i++)
      if (sceneItems.recognizedColoredIcons[i].color.equals(new Color(r,g,b)) )
        return true;
   
    return false;
  }
 
 
  /**
   * Remueve los �conos coloreados identificados con el color pasado por
   * parametro.
   * @param r    componentes <i>red</i> del color.
   * @param g    componentes <i>green</i> del color.
   * @param b    componentes <i>blue</i> del color.
   */
  public void removeRecognizedColoredIcon(int r, int g, int b)
  {
    int count = 0;
    int i , j;
   
    // Cuenta cu�ntos �conos fueron reconocidos con el mismo color.
    for (i=0; i<sceneItems.recognizedColoredIcons.length; i++)
      if (sceneItems.recognizedColoredIcons[i].color.equals(new Color(r,g,b)) )
        count++;
   
    // Achica el arreglo.
    RecognizedShape[] newArray = new RecognizedShape[sceneItems.recognizedColoredIcons.length - count] ;
   
    // Copia todos los dem�s
    for (i=0, j=0; i<sceneItems.recognizedColoredIcons.length; i++)
      if ( !sceneItems.recognizedColoredIcons[i].color.equals(new Color(r,g,b)) )
        // Distinto => lo copia.
        newArray[j++]=sceneItems.recognizedColoredIcons[i];
       
    sceneItems.recognizedColoredIcons = newArray;
  }
 
 
  public void renameRecongnizedColoredIcon(int r, int g, int b, String newName)
  {
    for (int i=0; i<sceneItems.recognizedColoredIcons.length; i++)
      if ( sceneItems.recognizedColoredIcons[i].color.equals(new Color(r,g,b)) )
        sceneItems.recognizedColoredIcons[i].shapeId = newName;
  }


  public void setImageCollector(ImageCollector ic)
  {
    this.ic = ic;
  }


  public ImageCollector getImageCollector()
  {
    return ic;
  }

 
  private synchronized void loadImage()
  {
    if ( ic != null ) {
      image = ic.grabImage();
    }
 
    if ( image != null )
      imageUtils.setImage( image );
  }

 
  public RobotState getRobotState(int r, int g, int b) throws GPSException
  {
    RobotState state = null;
    boolean realSimulationRecognizeFailed =false;
    try
    {
        recognizeRobotr , g , b );     
    }
    catch (GPSException e)
    {
      // Si la simulacion es real, si no se reconoce el color del robot no pasa nada
      if(Simulation.getCurrent().getType() != Defines.REAL_SIMULATION )
        throw new GPSException( e.getMessage() );
      else
        realSimulationRecognizeFailed = true;
    }
   
    gpsSemaphore.WAIT();
    if ( robot != null || realSimulationRecognizeFailed)
    {
      state = new RobotState();
      if(realSimulationRecognizeFailed) {
        state.position  = new double[] { 0 , 0 };
        state.course  = 1;
      } else {
        state.position  = new double[] { robot.x , robot.y };
        state.course  = Math.atan2( robot.y-robot.yOld , robot.x-robot.xOld );       
      }
    }
    gpsSemaphore.SIGNAL();
   
    return state;
  }

 

  public void setXMLConfigFile()
  {
    // TODO Auto-generated method stub
  }


  public void close()
  {
    if ( ic != null  &&  !ic.isClosed() )
      ic.close();
  }
}
TOP

Related Classes of components.gps.imagebased.ImageBasedGPS

TOP
Copyright © 2018 www.massapi.com. All rights reserved.
All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.