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
{
recognizeRobot( r , 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();
}
}