package components.commchannel.virtual;
import java.awt.Color;
import java.util.Hashtable;
import utils.defines.Defines;
import utils.logging.Logger;
import components.commchannel.CommunicationChannel;
import components.features.sensor.Sensor;
import components.features.sensor.color.ColorSensor;
import components.features.sensor.proximity.ProximitySensor;
import components.gps.GPS;
import components.gps.RecognizedItems;
import components.gps.imagebased.ImageBasedGPS;
import components.imagecollector.virtual.VirtualImageCollector;
import components.robot.Robot;
import components.robot.RobotState;
import core.simulation.Simulation;
/**
* Esta clase implementa un canal virtual (para comunicaci�n contra
* robots virtuales).
*/
public class VirtualCommunicationChannel extends CommunicationChannel
{
// Constantes
private static final int FRONT_SENSOR = 1;
private static final int LEFT_SENSOR = 2;
private static final int RIGHT_SENSOR = 3;
private static final int SLEEP_TIME = 5; // ms
// Atributos
private Robot robot;
private VirtualRobotState state;
private RecognizedItems sceneItems;
private double realCourse;
private final class VirtualRobotState
{
// Sensores
public double frontSensorX, frontSensorY;
public double leftSensorX, leftSensorY;
public double rightSensorX, rightSensorY;
public double frontX, frontY;
public double leftX, leftY;
public double rightX, rightY;
}
/**
* Construye un nuevo canal virtual de comunicaciones con nombre y
* descripci�n nulos, y no asociado a ning�n robot.
*/
public VirtualCommunicationChannel()
{
super();
this.state = new VirtualRobotState();
}
/**
* Construye un nuevo canal virtual de comunicaciones con nombre
* <code>name</code>, descripci�n <code>description</code>.
* @param name nombre.
* @param description descripci�n.
*/
public VirtualCommunicationChannel(String name, String description)
{
super(name, description);
this.state = new VirtualRobotState();
}
public int open(String connectionString)
{
if ( connectionString == null )
return Defines.ERROR;
super.open( null );
robot = Simulation.getCurrent().getRobotByName( connectionString );
if ( robot == null )
return Defines.ERROR;
setCommChannelSemaphore( robot.getCommChannelSemaphore() );
return Defines.OK;
}
public Hashtable receive()
{
if ( isClosed() )
return null;
return features.featuresHash;
}
public void send(String msg)
{
if ( isClosed() )
return;
if ( msg != null )
{
String[] fields = msg.split( "\\s*=\\s*" );
if ( fields[0].equalsIgnoreCase( "left" ) )
{
realCourse += 2.0*Math.PI - Double.parseDouble(fields[1]);
realCourse %= 2.0*Math.PI; // Normaliza el curso entre 0 y 2PI
}
else if ( fields[0].equalsIgnoreCase( "right" ) )
{
realCourse += Double.parseDouble(fields[1]);
realCourse %= 2.0*Math.PI; // Normaliza el curso entre 0 y 2PI
}
else if ( fields[0].equalsIgnoreCase( "advance" ) )
{
RobotState robotState = robot.getState();
Color color = new Color(robot.getColor().getRed() ,
robot.getColor().getGreen() ,
robot.getColor().getBlue() );
robotState.position[0] += Defines.STEP_SIZE * Math.cos(realCourse);
robotState.position[1] += Defines.STEP_SIZE * Math.sin(realCourse);
if( ((ImageBasedGPS) Simulation.getCurrent().getGps()).getImageCollector() instanceof VirtualImageCollector)
((VirtualImageCollector)((ImageBasedGPS) Simulation.getCurrent().getGps()).getImageCollector())
.drawRobot( color , robotState.position );
}
}
}
public boolean test()
{
return true;
}
public void run()
{
Sensor sensor;
RobotState robotState;
double[] pos;
double course;
boolean finished1, finished2, finished3;
int i;
Logger.info( "El canal de comunicaciones comienza su ejecuci�n." );
int width = ((ImageBasedGPS) Simulation.getCurrent().getGps()).getImageCollector().getImageResolution().width;
int height = ((ImageBasedGPS) Simulation.getCurrent().getGps()).getImageCollector().getImageResolution().height;
if ( width <= 0 ) width = Defines.DEFAULT_IMAGE_RESOLUTION[0];
if ( height <= 0 ) height = Defines.DEFAULT_IMAGE_RESOLUTION[1];
while ( ! isClosed() )
{
// Duerme durante un intervalo
try {
Thread.sleep( SLEEP_TIME );
} catch (InterruptedException e) { }
// TODO: cuidado con �sto!!! En este punto, lo �nico que me
// ayuda para que el sem�foro no sea null es el sleep() anterior
// (de otra forma, pinchar�a). Implementarlo bien!
getCommChannelSemaphore().WAIT();
if ( Simulation.getCurrent().getGps() != null )
sceneItems = Simulation.getCurrent().getGps().getMazeItems();
else
sceneItems = null;
if ( sceneItems == null )
{
Logger.error("No hay �tems reconocidos en el escenario.");
continue;
}
robotState = robot.getState();
pos = robotState.position;
course = realCourse;
// Calcula las coordenadas de los puntos laterales, y del punto frontal.
state.frontX = pos[0] + Defines.ROBOT_RADIO * Math.cos( course );
state.frontY = pos[1] + Defines.ROBOT_RADIO * Math.sin( course );
state.leftX = pos[0] + Defines.ROBOT_RADIO * Math.cos( course + 3.0*Math.PI/2.0);
state.leftY = pos[1] + Defines.ROBOT_RADIO * Math.sin( course + 3.0*Math.PI/2.0);
state.rightX = pos[0] + Defines.ROBOT_RADIO * Math.cos( course + Math.PI/2.0 );
state.rightY = pos[1] + Defines.ROBOT_RADIO * Math.sin( course + Math.PI/2.0 );
// Calcula los puntos de alcance de todos los sensores.
state.frontSensorX = state.frontX + Defines.FRONT_SENSOR_DISTANCE * Math.cos(course);
state.frontSensorY = state.frontY + Defines.FRONT_SENSOR_DISTANCE * Math.sin(course);
state.leftSensorX = state.leftX + Defines.LEFT_SENSOR_DISTANCE * Math.cos(Math.PI/2.0 - course);
state.leftSensorY = state.leftY - Defines.LEFT_SENSOR_DISTANCE * Math.sin(Math.PI/2.0 - course);
state.rightSensorX = state.rightX - Defines.RIGHT_SENSOR_DISTANCE * Math.cos(Math.PI/2.0 - course);
state.rightSensorY = state.rightY + Defines.RIGHT_SENSOR_DISTANCE * Math.sin(Math.PI/2.0 - course);
//////////////////////////////////////////
// //
// Se analiza el SENSOR FRONTAL //
// //
//////////////////////////////////////////
finished1 = false;
for ( i=0; i<sceneItems.recognizedColoredIcons.length && !finished1; i++)
{
if ( detect( FRONT_SENSOR , i, GPS.GPS_COLORS) )
// El sensor frontal detecta un �cono coloreado
finished1 = true;
}
finished2 = false;
for ( i=0; i<sceneItems.recognizedWalls.length && !finished2; i++)
{
if ( detect( FRONT_SENSOR , i, GPS.GPS_WALLS) )
// El sensor frontal detecta una pared
finished2 = true;
}
finished3 = false;
/*
for ( i=0; i<sceneItems.recognizedRobots.length && !finished3; i++)
{
if ( detect( FRONT_SENSOR , i, GPS.GPS_ROBOTS) )
// El sensor frontal detecta otro robot
finished3 = true;
}
*/
if ( finished1 || finished2 || finished3 )
{
// El sensor frontal est� detectando algo
sensor = (ProximitySensor)features.featuresHash.get( FRONT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(true) );
else
// TODO: ver c�mo manejar la situaci�n en que no se encuentra
// el feature en el hash.
Logger.error("No se pudo acceder al sensor frontal.");
}
else
{
// No se detecta ninguna pared al frente
sensor = (ProximitySensor)features.featuresHash.get( FRONT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(false) );
}
//////////////////////////////////////////
// //
// Se analiza el SENSOR IZQUIERDO //
// //
//////////////////////////////////////////
finished1 = false;
for ( i=0; i<sceneItems.recognizedColoredIcons.length && !finished1; i++)
{
if ( detect( LEFT_SENSOR , i, GPS.GPS_COLORS) )
// El sensor izquierdo detecta un �cono coloreado
finished1 = true;
}
finished2 = false;
for ( i=0; i<sceneItems.recognizedWalls.length && !finished2; i++)
{
if ( detect( LEFT_SENSOR , i, GPS.GPS_WALLS) )
// El sensor izquierdo detecta una pared
finished2 = true;
}
finished3 = false;
/*
for ( i=0; i<sceneItems.recognizedRobots.length && !finished3; i++)
{
if ( detect( LEFT_SENSOR , i, GPS.GPS_ROBOTS) )
// El sensor izquierdo detecta otro robot
finished3 = true;
}
*/
if ( finished1 || finished2 || finished3 )
{
// El sensor izquierdo est� detectando algo
sensor = (ProximitySensor)features.featuresHash.get( LEFT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(true) );
else
// TODO: ver c�mo manejar la situaci�n en que no se encuentra
// el feature en el hash.
Logger.error("No se pudo acceder al sensor izquierdo.");
}
else
{
// No se detecta ninguna pared a la izquierda
sensor = (ProximitySensor)features.featuresHash.get( LEFT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(false) );
else
// TODO: ver c�mo manejar la situaci�n en que no se encuentra
// el feature en el hash.
Logger.error("No se pudo acceder al sensor izquierdo.");
}
//////////////////////////////////////////
// //
// Se analiza el SENSOR DERECHO //
// //
//////////////////////////////////////////
finished1 = false;
for ( i=0; i<sceneItems.recognizedColoredIcons.length && !finished1; i++)
{
if ( detect( RIGHT_SENSOR , i, GPS.GPS_COLORS) )
// El sensor derecho detecta un �cono coloreado
finished1 = true;
}
finished2 = false;
for ( i=0; i<sceneItems.recognizedWalls.length && !finished2; i++)
{
if ( detect( RIGHT_SENSOR , i, GPS.GPS_WALLS) )
// El sensor derecho detecta una pared
finished2 = true;
}
finished3 = false;
/*
for ( i=0; i<sceneItems.recognizedRobots.length && !finished3; i++)
{
if ( detect( RIGHT_SENSOR , i, GPS.GPS_ROBOTS) )
// El sensor derecho detecta otro robot
finished3 = true;
}
*/
if ( finished1 || finished2 || finished3 )
{
// El sensor derecho est� detectando algo
sensor = (ProximitySensor)features.featuresHash.get( RIGHT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(true) );
// TODO: ver c�mo manejar la situaci�n en que no se encuentra
// el feature en el hash.
}
else
{
// No se detecta ninguna pared a la derecha
sensor = (ProximitySensor)features.featuresHash.get( RIGHT_SENSOR_NAME );
if ( sensor != null )
sensor.setValue( new Boolean(false) );
}
//////////////////////////////////////////
// //
// Se analiza el SENSOR DE COLOR //
// //
//////////////////////////////////////////
double xl = pos[0] + 0.5*Defines.ROBOT_RADIO * Math.cos( course + 3.0*Math.PI/2.0);
double yl = pos[1] + 0.5*Defines.ROBOT_RADIO * Math.sin( course + 3.0*Math.PI/2.0);
double xr = pos[0] + 0.5*Defines.ROBOT_RADIO * Math.cos( course + Math.PI/2.0 );
double yr = pos[1] + 0.5*Defines.ROBOT_RADIO * Math.sin( course + Math.PI/2.0 );
Color colorLeft = null;
Color colorRight= null;
// Se analiza el "flanco" izquierdo
finished1 = false;
finished2 = false;
finished3 = false;
while ( !finished1 && !finished2 && !finished3 )
{
xl += Defines.STEP_SIZE * Math.cos( course );
yl += Defines.STEP_SIZE * Math.sin( course );
for (i=0; !finished1 && i<sceneItems.recognizedWalls.length; i++)
if ( sceneItems.recognizedWalls[i].contains(xl, yl) )
// Hay una pared
finished1 = true;
// TODO: verificar que no est� chocando contra otro robot
for (i=0; !finished1 && !finished2 && !finished3 && i<sceneItems.recognizedColoredIcons.length; i++)
if ( sceneItems.recognizedColoredIcons[i].contains(xl, yl) )
{
// Encontr� un color
sensor = (ColorSensor)features.featuresHash.get( COLOR_SENSOR_NAME );
if ( sensor != null )
colorLeft = sceneItems.recognizedColoredIcons[i].color;
finished3 = true;
}
if ( xl < 0 || xl > width || yl < 0 || yl > height )
// Se fue del rango de la imagen...
break;
}
// Se analiza el "flanco" derecho
finished1 = false;
finished2 = false;
finished3 = false;
while ( !finished1 && !finished2 && !finished3 )
{
xr += Defines.STEP_SIZE * Math.cos( course );
yr += Defines.STEP_SIZE * Math.sin( course );
for (i=0; !finished1 && i<sceneItems.recognizedWalls.length; i++)
if ( sceneItems.recognizedWalls[i].contains(xr, yr) )
// Hay una pared
finished1 = true;
// TODO: verificar que no est� chocando contra otro robot
for (i=0; !finished1 && !finished2 && !finished3 && i<sceneItems.recognizedColoredIcons.length; i++)
if ( sceneItems.recognizedColoredIcons[i].contains(xr, yr) )
{
// Encontr� un color
sensor = (ColorSensor)features.featuresHash.get( COLOR_SENSOR_NAME );
if ( sensor != null )
colorRight = sceneItems.recognizedColoredIcons[i].color;
finished3 = true;
}
if ( xr < 0 || xr > width || yr < 0 || yr > height )
// Se fue del rango de la imagen...
break;
}
sensor = (ColorSensor)features.featuresHash.get( COLOR_SENSOR_NAME );
if ( colorLeft != null && colorRight != null && colorLeft.equals(colorRight) )
{
// Ambos "flancos" detectan el mismo color. Entonces, se considera
// que el robot est� viendo ese color.
if ( sensor != null )
sensor.setValue( colorLeft );
}
else
{
// No encontr� color
if ( sensor != null )
sensor.setValue( null );
}
getCommChannelSemaphore().SIGNAL();
}
Logger.info( "El canal de comunicaciones finaliza su ejecuci�n." );
}
/**
* Verifica si el sensor <code>sensor</code> est� detectando el �tem
* <code>mazeItem</code> de tipo <code>itemType</code>.
* @param sensor sensor a verificar (<code>FRONT_SENSOR</code>,
* <code>LEFT_SENSOR</code>, <code>RIGHT_SENSOR</code>,
* etc).
* @param mazeItem n�mero de �tem a verificar.
* @param itemType tipo de �tem.
* @return <code>true</code> si el sensor detecta el �tem, o
* <code>false</code> en caso contrario.
*/
private boolean detect(int sensor, int mazeItem, int itemType)
{
// Se calcula un punto intermedio en el rayo del sensor,
// para tener m�s precisi�n.
double middleX, middleY;
if ( sensor == FRONT_SENSOR )
{
middleX = (state.frontSensorX + state.frontX) / 2.0;
middleY = (state.frontSensorY + state.frontY) / 2.0;
}
else if ( sensor == LEFT_SENSOR )
{
middleX = (state.leftSensorX + state.leftX) / 2.0;
middleY = (state.leftSensorY + state.leftY) / 2.0;
}
else if ( sensor == RIGHT_SENSOR )
{
middleX = (state.rightSensorX + state.rightX) / 2.0;
middleY = (state.rightSensorY + state.rightY) / 2.0;
}
else
{
return false;
}
if ( itemType == GPS.GPS_WALLS )
{
if ( sensor == FRONT_SENSOR )
{
return ( sceneItems.recognizedWalls[mazeItem].contains(state.frontSensorX, state.frontSensorY) ||
sceneItems.recognizedWalls[mazeItem].contains(middleX, middleY) ||
sceneItems.recognizedWalls[mazeItem].contains(state.frontX, state.frontY) );
}
else if ( sensor == LEFT_SENSOR )
{
return ( sceneItems.recognizedWalls[mazeItem].contains(state.leftSensorX, state.leftSensorY) ||
sceneItems.recognizedWalls[mazeItem].contains(middleX, middleY) ||
sceneItems.recognizedWalls[mazeItem].contains(state.leftX, state.leftY) );
}
else if ( sensor == RIGHT_SENSOR )
{
return ( sceneItems.recognizedWalls[mazeItem].contains(state.rightSensorX, state.rightSensorY) ||
sceneItems.recognizedWalls[mazeItem].contains(middleX, middleY) ||
sceneItems.recognizedWalls[mazeItem].contains(state.rightX, state.rightY) );
}
}
else if ( itemType == GPS.GPS_COLORS )
{
if ( sensor == FRONT_SENSOR )
{
return ( sceneItems.recognizedColoredIcons[mazeItem].contains(state.frontSensorX, state.frontSensorY) ||
sceneItems.recognizedColoredIcons[mazeItem].contains(middleX, middleY) ||
sceneItems.recognizedColoredIcons[mazeItem].contains(state.frontX, state.frontY) );
}
else if ( sensor == LEFT_SENSOR )
{
return ( sceneItems.recognizedColoredIcons[mazeItem].contains(state.leftSensorX, state.leftSensorY) ||
sceneItems.recognizedColoredIcons[mazeItem].contains(middleX, middleY) ||
sceneItems.recognizedColoredIcons[mazeItem].contains(state.leftX, state.leftY) );
}
else if ( sensor == RIGHT_SENSOR )
{
return ( sceneItems.recognizedColoredIcons[mazeItem].contains(state.rightSensorX, state.rightSensorY) ||
sceneItems.recognizedColoredIcons[mazeItem].contains(middleX, middleY) ||
sceneItems.recognizedColoredIcons[mazeItem].contains(state.rightX, state.rightY) );
}
}
else if ( itemType == GPS.GPS_ROBOTS )
{
// TODO: implementar
}
return false;
}
public String getXMLConfig(String ident)
{
String xmlConfig = new String();
String tabulator = new String();
tabulator = tabulator.concat(ident);
xmlConfig = xmlConfig.concat(tabulator + "<communicationchannel name=\""+getName()+"\" description=\""+getDescription()+"\" />");
return xmlConfig;
}
}