package components.robot.mazeresolver;
import java.awt.Color;
import java.util.Random;
import utils.defines.Defines;
import utils.images.RecognizedShape;
import utils.logging.Logger;
import components.commchannel.CommunicationChannel;
import components.commchannel.CommunicationChannelException;
import components.features.Feature;
import components.features.cam.Camera;
import components.features.engine.Engine;
import components.features.sensor.Sensor;
import components.gps.imagebased.ImageBasedGPS;
import components.imagecollector.virtual.VirtualImageCollector;
import components.robot.Robot;
import components.robot.RobotException;
import components.robot.RobotState;
import core.simulation.Simulation;
/**
* <code>MazeResolverRobot</code> representa la clase base para cualquier
* robot (real o virtual) capaz de resolver un laberinto. Esta clase implementa
* toda la l�gica de navegaci�n del robot en el laberinto, para lo cual toma
* datos de los <i>features</i> (por ejemplo, sensores) que lo conforman.
* No interesa si estos sensores son simulados por software (en el caso de un
* robot virtual), o si son componentes electr�nicos (en un robot real), como
* tampoco interesa la forma en que se le dan �rdenes al robot.
* Todos esos son detalles que resuelve el canal de comunicaciones asociado
* (solo es necesario conocer el protocolo para enviar y recibir datos a
* trav�s de �l).
*/
public class MazeResolverRobot extends Robot
{
// Atributos para control de movimiento
private java.awt.Color viewedColor, lastViewColor;
private Random randNums;
private boolean move, leftRotating, rightRotating;
private int leftRotations, rightRotations;
/* Variables nueva navegacion */
private static final int UNRECOGNIZED_PLACE = -634;
enum NAV_STATE{AVANZANDO,
BUSCANDO_PROXIMO_LUGAR,
OBTENIENDO_VISTAS_INICIANDO,
OBTENIENDO_VISTAS_EXPLORANDO,
CENTRANDO_PROXIMO_LUGAR_IZQUIERDA,
CENTRANDO_PROXIMO_LUGAR_DERECHA,
EVITANDO_PARED
};
private NAV_STATE nav_state;
private int nav_current_place;
private int nav_next_place;
private boolean nav_center_place_enabled;
private int nav_current_views[];
private int nav_last_viewed_place;
/**
* Construye un nuevo robot resolvedor de laberintos, con nombre nulo,
* posici�n inicial (0.0,0.0), y direcci�n inicial 0.0 rad.
*/
public MazeResolverRobot()
{
super();
randNums = new Random();
move = true;
leftRotating = false;
rightRotating = false;
leftRotations = 0;
rightRotations = 0;
statusTable.put(new Integer(Defines.STATE_ROBOT_NOT_CALIBRATED),"Sin posicionar");
statusTable.put(new Integer(Defines.STATE_ROBOT_CALIBRATED), "Posicionado!");
}
private final double computeDeltaNewCourse(int currentPlace)
{
RobotState state = getState();
double newCourse = 0.0;
int nextPlaceId;
if ( currentPlace != neuralNet.getCurrentPlace() )
{
// La red neuronal "piensa" que estamos en un lugar, pero en
// realidad, el robot est� en otro lugar diferente. Esto puede
// suceder por las imperfecciones en la navegaci�n del robot.
// TODO: ��� c�mo manejar esta situaci�n ???
Logger.warn( "Lugar actual en la red neuronal: " +
neuralNet.getCurrentPlace() +
" Lugar actual real del robot: " +
currentPlace );
}
neuralNet.move( Simulation.getCurrent().getPlaces()[neuralNet.getCurrentPlace()-1] , currentPlace );
nextPlaceId = neuralNet.getCurrentPlace();
Logger.info( "Actual: " + currentPlace + " Pr�ximo: " + nextPlaceId );
// Busca el lugar pr�ximo en el arreglo de �conos coloreados.
boolean encontrado = false;
for (int i=0; !encontrado && i<Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons.length; i++)
{
RecognizedShape icon = Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons[i];
if ( icon.shapeId.equals(String.valueOf(nextPlaceId)) )
{
// Establece el nuevo curso
newCourse = Math.atan2( icon.y-state.position[1] , icon.x-state.position[0] ) - state.course;
encontrado = true;
}
}
if ( !encontrado )
{
// TODO: qu� hacer si no se encuentra el �cono coloreado?
// Por ahora, se mantiene el mismo curso.
Logger.error( "No se encontr� el �cono coloreado con id=" + nextPlaceId );
}
return newCourse;
}
/**
* Implementa la l�gica de navegaci�n del robot en el laberinto,
* abstray�ndose de los <i>features</i> (sensores).
* Este m�todo no debe sobreescribirse en las clases hijas.
*/
final int currentlyViewedPlace() {
java.awt.Color viewedColor = (java.awt.Color)((Sensor)commChannel.receive().get(CommunicationChannel.COLOR_SENSOR_NAME)).getValue();
if(viewedColor == null ) return UNRECOGNIZED_PLACE;
RecognizedShape[] icons = Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons;
for (int i=0; i<icons.length; i++)
if ( icons[i].color.equals(viewedColor) )
return Integer.valueOf(icons[i].shapeId).intValue();
return UNRECOGNIZED_PLACE;
}
public final void move() throws RobotException
{
if(firstMovement) {
nav_current_place=UNRECOGNIZED_PLACE;
nav_next_place=1;
nav_last_viewed_place=1;
nav_center_place_enabled=false;
nav_state=NAV_STATE.AVANZANDO;
firstMovement=false;
nav_current_views=new int[Simulation.getCurrent().getPlaces()[0].length];
}
Boolean viewing_wall = ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue();
int viewed_place = currentlyViewedPlace();
if(viewed_place != UNRECOGNIZED_PLACE) {
// El ultimo lugar que identifique
nav_last_viewed_place = viewed_place;
}
switch(nav_state) {
case AVANZANDO:
//System.out.println("AVANZANDO");
if(viewing_wall) {
if(viewed_place != UNRECOGNIZED_PLACE) {
// Estaba avanzando y encontro un lugar
if(nav_next_place != viewed_place) {
// La red neuronal "piensa" que estamos en un lugar, pero en
// realidad, el robot est� en otro lugar diferente. Esto puede
// suceder por las imperfecciones en la navegaci�n del robot.
Logger.warn( "Lugar actual en la red neuronal: "
+ nav_next_place
+ " Lugar actual real del robot: "
+ viewed_place);
nav_next_place = viewed_place;
}
nav_current_place = nav_next_place;
if ( nav_current_place == Simulation.getCurrent().getTargetPlace() ) {
// Se lleg� al objetivo
reset();
}
else {
nav_state = NAV_STATE.OBTENIENDO_VISTAS_INICIANDO;
}
}
else {
// Choco contra una pared
nav_state = NAV_STATE.EVITANDO_PARED;
}
}
else {
if(viewed_place != nav_next_place) {
// Perdio de vista el proximo lugar
nav_state = NAV_STATE.BUSCANDO_PROXIMO_LUGAR;
}
else {
advance();
}
}
break;
case EVITANDO_PARED:
if(viewing_wall) {
turnLeft( Defines.DELTA_PHI_ROTATIONS );
}
else {
nav_state = NAV_STATE.BUSCANDO_PROXIMO_LUGAR;
}
break;
case BUSCANDO_PROXIMO_LUGAR:
//System.out.println("BUSCANDO_PROXIMO_LUGAR");
// TODO: �Que pasa si desde donde esta no puede ver el proximo lugar?
// �Puede pasar esta situacion?
if(viewed_place == nav_next_place) {
if(nav_center_place_enabled) {
// Hacemos que el robot se mueva hacia el centro de lo que esta viendo
// primero rotamos a la izquierda hasta no ver ese color (CENTRANDO A IZQUIERDA)
// y luego rotamos a la derecha la mitad de veces que rotamos a izquierda (CENTRANDO A DERECHA)
leftRotations=0;
rightRotations=0;
nav_state = NAV_STATE.CENTRANDO_PROXIMO_LUGAR_IZQUIERDA;
}
else {
nav_state = NAV_STATE.AVANZANDO;
}
}
else {
if(leftRotations*Defines.DELTA_PHI_ROTATIONS > 2*Math.PI) {
// No veo mi lugar objetivo, me perdi
// Probablemente ni siquiera se donde estoy
nav_next_place = nav_last_viewed_place;
leftRotations=0;
}
else {
turnLeft( Defines.DELTA_PHI_ROTATIONS );
leftRotations++;
}
}
break;
case OBTENIENDO_VISTAS_INICIANDO:
if(viewed_place == nav_current_place) {
turnRight( Defines.DELTA_PHI_ROTATIONS );
}
else {
// Antes de explorar vistas, limpio el arreglo
for(int i=0;i<nav_current_views.length;i++) {
nav_current_views[i] = 0;
}
nav_state = NAV_STATE.OBTENIENDO_VISTAS_EXPLORANDO;
}
break;
case OBTENIENDO_VISTAS_EXPLORANDO:
if(viewed_place == nav_current_place) {
// Gire 360� o algo parecido
// Invoco a la red neuronal para determinar el proximo lugar
System.out.println("[ROBOT] --- INVOCO RED NEURONAL ---");
neuralNet.move( nav_current_views , nav_current_place );
nav_next_place = neuralNet.getCurrentPlace();
nav_state = NAV_STATE.BUSCANDO_PROXIMO_LUGAR;
}
else {
if(viewed_place != UNRECOGNIZED_PLACE) {
// Marco el color como que lo estoy viendo desde el lugar actual
// Esta operacion es idempotente, o sea, no importa si lo marco N veces o 1 sola vez.
if(nav_current_views[viewed_place-1]==0) {
System.out.println("[ROBOT] Veo el lugar:"+viewed_place);
}
nav_current_views[viewed_place-1] = 1;
}
turnRight( Defines.DELTA_PHI_ROTATIONS );
}
break;
case CENTRANDO_PROXIMO_LUGAR_IZQUIERDA:
//System.out.println("CENTRANDO A IZQUIERDA");
if(viewed_place == nav_next_place) {
turnLeft( Defines.DELTA_PHI_ROTATIONS );
leftRotations++;
}
else {
//System.out.println("CENTRANDO A IZQUIERDA => A DERECHA");
nav_state = NAV_STATE.CENTRANDO_PROXIMO_LUGAR_DERECHA;
}
break;
case CENTRANDO_PROXIMO_LUGAR_DERECHA:
//System.out.println("CENTRANDO A DERECHA");
if(rightRotations <= leftRotations/2) {
turnRight( Defines.DELTA_PHI_ROTATIONS );
rightRotations++;
}
else if(viewed_place == nav_next_place) {
//System.out.println("CENTRANDO A DERECHA => AVANZAR");
nav_state = NAV_STATE.AVANZANDO;
}
else {
// No deberia pasar
turnRight( Defines.DELTA_PHI_ROTATIONS );
}
break;
} // fin switch
}
/**
* move original de EasyBot
*/
@Deprecated
private final void _move_original() throws RobotException
{
// Avanza
if ( move )
{
try
{
advance();
updateState();
}
catch (RobotException e)
{
Logger.error( e.getMessage() );
}
}
move = false;
if ( firstMovement )
{
turnRight( computeDeltaNewCourse( 1 ) );
firstMovement = false;
}
else if ( leftRotating )
{
// Est� rotando a izquierda
boolean aux = ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue();
if ( !aux )
{
// Libr�
leftRotating = false;
if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
{
// Ahora empieza a girar a derecha
rightRotating = true;
turnRight( Defines.DELTA_PHI_ROTATIONS );
rightRotations++;
}
else
{
// Simulaci�n real. Entonces empieza a avanzar.
leftRotations = 0;
move = true;
}
}
else
{
turnLeft( Defines.DELTA_PHI_ROTATIONS );
leftRotations++;
}
}
else if ( rightRotating )
{
// Est� girando a derecha
if ( ! ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue() )
{
// Libr�
leftRotating = false;
rightRotating = false;
rightRotations -= leftRotations;
if ( leftRotations < rightRotations )
{
// Gira a izquierda
for (int i=0; i<leftRotations+rightRotations/*+1*/; i++)
turnLeft( Defines.DELTA_PHI_ROTATIONS );
}
leftRotations = 0;
rightRotations = 0;
}
else
{
turnRight( Defines.DELTA_PHI_ROTATIONS );
rightRotations++;
}
}
else if ( (viewedColor=(java.awt.Color)((Sensor)commChannel.receive().get(CommunicationChannel.COLOR_SENSOR_NAME)).getValue()) != null &&
((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue() &&
( lastViewColor == null || !viewedColor.equals(lastViewColor)) )
{
// Est� percibiendo un color.
int currentPlace = -1;
RecognizedShape[] icons = Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons;
for (int i=0; currentPlace==-1 && i<icons.length; i++)
if ( icons[i].color.equals(viewedColor) )
currentPlace = Integer.valueOf(icons[i].shapeId).intValue();
int[][] places = Simulation.getCurrent().getPlaces();
if ( currentPlace != -1 && places != null )
{
// Calcula el nuevo curso
turnRight( computeDeltaNewCourse( currentPlace ) );
if ( currentPlace == Simulation.getCurrent().getTargetPlace() )
// Se lleg� al objetivo
reset();
}
else
{
// TODO: ver c�mo manejar la situaci�n en que no
// se puede resolver el lugar
// Por ahora, asume que si no puede resolver el lugar,
// contin�a avanzando hacia una posici�n aleatoria
turnRight( randNums.nextDouble() * 2.0*Math.PI );
}
}
else if ( ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue() )
{
// Est� percibiendo una pared en el frente.
// Comienza el proceso de rotaci�n.
leftRotating = true;
rightRotating = false;
assert leftRotations == 0 : "leftRotations = "+leftRotations;
turnLeft( Defines.DELTA_PHI_ROTATIONS );
leftRotations++;
}
else if ( ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.LEFT_SENSOR_NAME)).getValue()).booleanValue() )
{
// Est� percibiendo una pared en la izquierda
turnRight( randNums.nextDouble() * 0.5*Defines.DELTA_PHI_ROTATIONS );
/*
// SOLO AVANZAR EN CONDICIONES NORMALES
if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
move = true;
*/
}
else if ( ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.RIGHT_SENSOR_NAME)).getValue()).booleanValue() )
{
// Est� percibiendo una pared en la derecha
turnLeft( randNums.nextDouble() * 0.5*Defines.DELTA_PHI_ROTATIONS );
/*
// SOLO AVANZAR EN CONDICIONES NORMALES
if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
move = true;
*/
}
else
{
// En condiciones normales, avanza.
move = true;
}
}
public void turnLeft(double angle) throws RobotException
{
commChannel.send( "left=" + angle );
update_logical_course(-angle,false);
updateState();
}
public void turnRight(double angle) throws RobotException
{
commChannel.send( "right=" + angle );
update_logical_course(+angle,false);
updateState();
}
public void advance() throws RobotException
{
commChannel.send( "advance" );
update_logical_course(0,true);
updateState();
}
/**
* Reinicializa el robot a su estado original, y se invoca al m�todo
* {@link components.neuralnetwork.NeuralNetwork#reset()
* reset()} de la red neuronal.
* @throws RobotException
*/
public void reset() throws RobotException
{
// Se reinicializa la red neuronal.
if ( neuralNet != null )
neuralNet.reset();
if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
{
// La simulaci�n es virtual (por lo tanto, el robot
// tambi�n lo es). Entonces, se lo vuelve a la posici�n
// original.
((VirtualImageCollector)((ImageBasedGPS) Simulation.getCurrent().getGps()).getImageCollector())
.drawRobot(
new Color(color.getRed(), color.getGreen(), color.getBlue()) ,
new double[] {initPosition[0], initPosition[1]} );
firstUpdate = true;
updateState();
Logger.info( "El robot virtual lleg� al objetivo." );
}
else
{
// TODO: qu� hacer si se lleg� al objetivo, y es
// un robot real ???
Logger.info( "El robot real lleg� al objetivo." );
}
firstMovement = true;
}
// TODO: ver este m�todo!!! (deber�a estar implementado de una manera
// m�s gen�rica en la clase ancestra Robot).
public void addFeature(Feature feature) throws RobotException
{
if ( features == null )
features = new Feature[0];
// Append
Feature[] temp = features;
features = new Feature[ temp.length + 1 ];
for (int i=0; i<temp.length; i++)
features[i] = temp[i];
features[ features.length - 1 ] = feature;
try
{
commChannel.registerFeature( feature );
}
catch (CommunicationChannelException e)
{
Logger.error( e.getMessage() );
throw new RobotException( e.getMessage() );
}
}
public String getXMLConfig(String ident)
{
String xmlConfig = new String();
String tabulator = new String();
tabulator = ident;
xmlConfig = xmlConfig.concat(tabulator + "<robot name=\""+ name + "\" description=\""+ description +"\">" + Defines.CR);
tabulator = ident + "\t";
xmlConfig = xmlConfig.concat(tabulator+ "<color>" + Defines.CR);
tabulator = ident + "\t\t";
xmlConfig = xmlConfig.concat(tabulator+ "<r>"+color.getRed()+"</r>" + Defines.CR);
xmlConfig = xmlConfig.concat(tabulator+ "<g>"+color.getGreen()+"</g>" + Defines.CR);
xmlConfig = xmlConfig.concat(tabulator+ "<b>"+color.getBlue()+"</b>" + Defines.CR);
tabulator = ident + "\t";
xmlConfig = xmlConfig.concat(tabulator+ "</color>" + Defines.CR);
xmlConfig = xmlConfig.concat(tabulator+ "<position>"+Defines.CR);
tabulator= ident + "\t\t";
xmlConfig = xmlConfig.concat(tabulator + "<x>"+initPosition[0]+"</x>" + Defines.CR);
xmlConfig = xmlConfig.concat(tabulator + "<y>"+initPosition[1]+"</y>" + Defines.CR);
tabulator= ident + "\t";
xmlConfig = xmlConfig.concat(tabulator+ "</position>" + Defines.CR);
xmlConfig = xmlConfig.concat(commChannel.getXMLConfig(tabulator)+Defines.CR);
xmlConfig = xmlConfig.concat(neuralNet.getXMLConfig(tabulator)+Defines.CR);
xmlConfig = xmlConfig.concat(tabulator+ "<features>"+Defines.CR);
tabulator = ident + "\t\t";
//Bloque de Sensores
xmlConfig = xmlConfig.concat(tabulator+ "<sensors>"+Defines.CR);
tabulator = ident + "\t\t\t";
for (int i=0 ; i<features.length;i++)
{
if (features[i] instanceof Sensor)
xmlConfig = xmlConfig.concat(features[i].getXMLConfig(tabulator));
}
tabulator = ident + "\t\t";
xmlConfig = xmlConfig.concat(tabulator+ "</sensors>"+Defines.CR);
//Bloques para engines
xmlConfig = xmlConfig.concat(tabulator+ "<engines>"+Defines.CR);
tabulator = ident + "\t\t\t";
for (int i=0 ; i<features.length;i++)
{
if (features[i] instanceof Engine)
xmlConfig = xmlConfig.concat(features[i].getXMLConfig(tabulator)+Defines.CR);
}
tabulator = ident + "\t\t";
xmlConfig = xmlConfig.concat(tabulator+ "</engines>"+Defines.CR);
//Bloques para camaras
xmlConfig = xmlConfig.concat(tabulator+ "<cameras>"+Defines.CR);
tabulator = ident + "\t\t\t";
for (int i=0 ; i<features.length;i++)
{
if (features[i] instanceof Camera)
xmlConfig = xmlConfig.concat(features[i].getXMLConfig(tabulator)+Defines.CR);
}
tabulator = ident + "\t\t";
xmlConfig = xmlConfig.concat(tabulator + "</cameras>"+Defines.CR);
//TODO:Bloques para misc
tabulator = ident + "\t";
xmlConfig = xmlConfig.concat(tabulator + "</features>" + Defines.CR);
xmlConfig = xmlConfig.concat("</robot>");
return xmlConfig;
}
}