Package components.commchannel.virtual

Source Code of components.commchannel.virtual.VirtualCommunicationChannel$VirtualRobotState

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 < ||  xl > width  ||  yl < ||  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 < ||  xr > width  ||  yr < ||  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;
  }
 
}
TOP

Related Classes of components.commchannel.virtual.VirtualCommunicationChannel$VirtualRobotState

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.